mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge joint bug fix into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2017-07-04 07:10:10 +02:00
commit 462fc1dfae
3 changed files with 117 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
.travis.yml
View File

@ -1,20 +1,42 @@
language: cpp
os:
- linux
- osx
compiler:
- gcc
- clang
install:
- if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- gcc-4.8
- g++-4.8
- clang
matrix:
# Linux / GCC
include:
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
packages:
- g++-4.9
env:
- MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
# OS X / GCC
- os: osx
osx_image: xcode8
env:
- MATRIX_EVAL="CC=gcc-4.9 && CXX=g++-4.9"
# Linux / Clang
- os: linux
addons:
apt:
sources:
- ubuntu-toolchain-r-test
- llvm-toolchain-precise-3.6
packages:
- clang-3.6
env:
- MATRIX_EVAL="CC=clang-3.6 && CXX=clang++-3.6"
# OS X / Clang
- os: osx
osx_image: xcode8
before_install:
- eval "${MATRIX_EVAL}"
branches:
only:
- master

52
src/constraint/FixedJoint.cpp
View File

@ -42,11 +42,18 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse();
// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
//
// q20 = q10 r0
// <=> r0 = q10^-1 q20
// <=> r0^-1 = q20^-1 q10
//
// where:
//
// q20 = initial orientation of body 2
// q10 = initial orientation of body 1
// r0 = initial rotation rotation from body 1 to body 2
mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation();
}
// Initialize before solving the constraint
@ -104,10 +111,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
// Compute the bias "b" for the 3 rotation constraints
mBiasRotation.setToZero();
if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
mBiasRotation = biasFactor * decimal(2.0) * qError.getVectorV();
}
@ -295,10 +301,32 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
mInverseMassMatrixRotation = mInverseMassMatrixRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints

51
src/constraint/SliderJoint.cpp
View File

@ -51,11 +51,18 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies
mInitOrientationDifferenceInv = transform2.getOrientation() *
transform1.getOrientation().getInverse();
mInitOrientationDifferenceInv.normalize();
mInitOrientationDifferenceInv.inverse();
// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
//
// q20 = q10 r0
// <=> r0 = q10^-1 q20
// <=> r0^-1 = q20^-1 q10
//
// where:
//
// q20 = initial orientation of body 2
// q10 = initial orientation of body 1
// r0 = initial rotation rotation from body 1 to body 2
mInitOrientationDifferenceInv = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Compute the slider axis in local-space of body 1
mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() *
@ -157,9 +164,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the bias "b" of the rotation constraint
mBRotation.setToZero();
if (mPositionCorrectionTechnique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
const Quaternion qError = orientationBody2 * mInitOrientationDifferenceInv * orientationBody1.getInverse();
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
}
@ -539,10 +544,32 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse();
}
// Compute the position error for the 3 rotation constraints
Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize();
const Quaternion qError = currentOrientationDifference * mInitOrientationDifferenceInv;
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mInitOrientationDifferenceInv * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints