Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp')
-rw-r--r--extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp699
1 files changed, 327 insertions, 372 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 76a1509471e..aa6f69000d2 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#include "btHingeConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
@@ -21,8 +20,6 @@ subject to the following restrictions:
#include <new>
#include "btSolverBody.h"
-
-
//#define HINGE_USE_OBSOLETE_SOLVER false
#define HINGE_USE_OBSOLETE_SOLVER false
@@ -30,59 +27,60 @@ subject to the following restrictions:
#ifndef __SPU__
-
-
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
- const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
- :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
+ const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
+ : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
#ifdef _BT_USE_CENTER_LIMIT_
- m_limit(),
+ m_limit(),
#endif
- m_angularOnly(false),
- m_enableAngularMotor(false),
- m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
- m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
- m_useReferenceFrameA(useReferenceFrameA),
- m_flags(0),
- m_normalCFM(0),
- m_normalERP(0),
- m_stopCFM(0),
- m_stopERP(0)
+ m_angularOnly(false),
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+ m_useReferenceFrameA(useReferenceFrameA),
+ m_flags(0),
+ m_normalCFM(0),
+ m_normalERP(0),
+ m_stopCFM(0),
+ m_stopERP(0)
{
m_rbAFrame.getOrigin() = pivotInA;
-
+
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
- if (projection >= 1.0f - SIMD_EPSILON) {
+ if (projection >= 1.0f - SIMD_EPSILON)
+ {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
- } else if (projection <= -1.0f + SIMD_EPSILON) {
+ }
+ else if (projection <= -1.0f + SIMD_EPSILON)
+ {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
- rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
- } else {
+ rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+ }
+ else
+ {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
- m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
- rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
- rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+ m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+ rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+ rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
+
+ btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+ btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
+ btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
- btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
- btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
- btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
-
m_rbBFrame.getOrigin() = pivotInB;
- m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
- rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
- rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-
-#ifndef _BT_USE_CENTER_LIMIT_
+ m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+ rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+ rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -94,47 +92,44 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
+ : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
#ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+ m_limit(),
#endif
-m_angularOnly(false), m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+ m_angularOnly(false),
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+ m_useReferenceFrameA(useReferenceFrameA),
+ m_flags(0),
+ m_normalCFM(0),
+ m_normalERP(0),
+ m_stopCFM(0),
+ m_stopERP(0)
{
-
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
m_rbAFrame.getOrigin() = pivotInA;
- m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
- rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
- rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+ m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
+ rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
+ rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
- btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
- btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
+ btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
+ btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
-
m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
- m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
- rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
- rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-
-#ifndef _BT_USE_CENTER_LIMIT_
+ m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
+ rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
+ rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
+
+#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -146,26 +141,24 @@ m_stopERP(0)
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
- const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
+ const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
+ : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
#ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+ m_limit(),
#endif
-m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+ m_angularOnly(false),
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+ m_useReferenceFrameA(useReferenceFrameA),
+ m_flags(0),
+ m_normalCFM(0),
+ m_normalERP(0),
+ m_stopCFM(0),
+ m_stopERP(0)
{
-#ifndef _BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -175,30 +168,28 @@ m_stopERP(0)
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
-}
-
-
+}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+ : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
#ifdef _BT_USE_CENTER_LIMIT_
-m_limit(),
+ m_limit(),
#endif
-m_angularOnly(false),
-m_enableAngularMotor(false),
-m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
-m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
-m_useReferenceFrameA(useReferenceFrameA),
-m_flags(0),
-m_normalCFM(0),
-m_normalERP(0),
-m_stopCFM(0),
-m_stopERP(0)
+ m_angularOnly(false),
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
+ m_useReferenceFrameA(useReferenceFrameA),
+ m_flags(0),
+ m_normalCFM(0),
+ m_normalERP(0),
+ m_stopCFM(0),
+ m_stopERP(0)
{
///not providing rigidbody B means implicitly using worldspace for body B
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
-#ifndef _BT_USE_CENTER_LIMIT_
+#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -210,9 +201,7 @@ m_stopERP(0)
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
-
-
-void btHingeConstraint::buildJacobian()
+void btHingeConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
{
@@ -221,8 +210,8 @@ void btHingeConstraint::buildJacobian()
if (!m_angularOnly)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3];
@@ -232,23 +221,23 @@ void btHingeConstraint::buildJacobian()
}
else
{
- normal[0].setValue(btScalar(1.0),0,0);
+ normal[0].setValue(btScalar(1.0), 0, 0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
- for (int i=0;i<3;i++)
+ for (int i = 0; i < 3; i++)
{
new (&m_jac[i]) btJacobianEntry(
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- pivotAInW - m_rbA.getCenterOfMassPosition(),
- pivotBInW - m_rbB.getCenterOfMassPosition(),
- normal[i],
- m_rbA.getInvInertiaDiagLocal(),
- m_rbA.getInvMass(),
- m_rbB.getInvInertiaDiagLocal(),
- m_rbB.getInvMass());
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ pivotAInW - m_rbA.getCenterOfMassPosition(),
+ pivotBInW - m_rbB.getCenterOfMassPosition(),
+ normal[i],
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbA.getInvMass(),
+ m_rbB.getInvInertiaDiagLocal(),
+ m_rbB.getInvMass());
}
}
@@ -258,60 +247,55 @@ void btHingeConstraint::buildJacobian()
//this is unused for now, it's a todo
btVector3 jointAxis0local;
btVector3 jointAxis1local;
-
- btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+
+ btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-
- new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
-
- new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
-
- new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
-
- // clear accumulator
- m_accLimitImpulse = btScalar(0.);
-
- // test angular limit
- testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
- //Compute K = J*W*J' for hinge axis
- btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
- m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
- getRigidBodyB().computeAngularImpulseDenominator(axisA));
+ new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
- }
-}
+ new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
+ new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
-#endif //__SPU__
+ // clear accumulator
+ m_accLimitImpulse = btScalar(0.);
+ // test angular limit
+ testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
-static inline btScalar btNormalizeAnglePositive(btScalar angle)
-{
- return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI));
+ //Compute K = J*W*J' for hinge axis
+ btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+ m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+ getRigidBodyB().computeAngularImpulseDenominator(axisA));
+ }
}
+#endif //__SPU__
+static inline btScalar btNormalizeAnglePositive(btScalar angle)
+{
+ return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
+}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
- btNormalizeAnglePositive(accAngle)));
+ btNormalizeAnglePositive(accAngle)));
return result;
}
@@ -320,41 +304,36 @@ static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
btScalar tol(0.3);
btScalar result = btShortestAngularDistance(accAngle, curAngle);
- if (btFabs(result) > tol)
+ if (btFabs(result) > tol)
return curAngle;
- else
+ else
return accAngle + result;
return curAngle;
}
-
btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
{
btScalar hingeAngle = getHingeAngle();
- m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle);
+ m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
return m_accumulatedAngle;
}
-void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
+void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
{
- m_accumulatedAngle = accAngle;
+ m_accumulatedAngle = accAngle;
}
void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
{
//update m_accumulatedAngle
btScalar curHingeAngle = getHingeAngle();
- m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle);
+ m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
btHingeConstraint::getInfo1(info);
-
}
-
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
-
-
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
@@ -362,17 +341,16 @@ void btHingeConstraint::getInfo1(btConstraintInfo1* info)
}
else
{
- info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
- info->nub = 1;
+ info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+ info->nub = 1;
//always add the row, to avoid computation (data is not available yet)
//prepare constraint
- testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
- if(getSolveLimit() || getEnableAngularMotor())
+ testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
+ if (getSolveLimit() || getEnableAngularMotor())
{
- info->m_numConstraintRows++; // limit 3rd anguar as well
- info->nub--;
+ info->m_numConstraintRows++; // limit 3rd anguar as well
+ info->nub--;
}
-
}
}
@@ -386,41 +364,38 @@ void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
else
{
//always add the 'limit' row, to avoid computation (data is not available yet)
- info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
- info->nub = 0;
+ info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
+ info->nub = 0;
}
}
-void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
+void btHingeConstraint::getInfo2(btConstraintInfo2* info)
{
- if(m_useOffsetForConstraintFrame)
+ if (m_useOffsetForConstraintFrame)
{
- getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+ getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
}
else
{
- getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
+ getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
}
}
-
-void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
{
///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
- testLimit(transA,transB);
+ testLimit(transA, transB);
- getInfo2Internal(info,transA,transB,angVelA,angVelB);
+ getInfo2Internal(info, transA, transB, angVelA, angVelB);
}
-
-void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
{
-
btAssert(!m_useSolveConstraintObsolete);
int i, skip = info->rowskip;
// transforms in world space
- btTransform trA = transA*m_rbAFrame;
- btTransform trB = transB*m_rbBFrame;
+ btTransform trA = transA * m_rbAFrame;
+ btTransform trB = transB * m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
@@ -448,7 +423,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_constraintError[i*skip]=0.f;
}
}
-#endif //#if 0
+#endif //#if 0
// linear (all fixed)
if (!m_angularOnly)
@@ -460,10 +435,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[skip + 1] = -1;
info->m_J2linearAxis[2 * skip + 2] = -1;
- }
-
-
-
+ }
btVector3 a1 = pivotAInW - transA.getOrigin();
{
@@ -471,22 +443,22 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
btVector3 a1neg = -a1;
- a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
btVector3 a2 = pivotBInW - transB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
- a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// linear RHS
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
- btScalar k = info->fps * normalErp;
+ btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
- for(i = 0; i < 3; i++)
+ for (i = 0; i < 3; i++)
{
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
}
@@ -504,9 +476,9 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
// get 2 orthos to hinge axis (X, Y)
btVector3 p = trA.getBasis().getColumn(0);
btVector3 q = trA.getBasis().getColumn(1);
- // set the two hinge angular rows
- int s3 = 3 * info->rowskip;
- int s4 = 4 * info->rowskip;
+ // set the two hinge angular rows
+ int s3 = 3 * info->rowskip;
+ int s4 = 4 * info->rowskip;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
@@ -521,185 +493,172 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
- // compute the right hand side of the constraint equation. set relative
- // body velocities along p and q to bring the hinge back into alignment.
- // if ax1,ax2 are the unit length hinge axes as computed from body1 and
- // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
- // if `theta' is the angle between ax1 and ax2, we need an angular velocity
- // along u to cover angle erp*theta in one step :
- // |angular_velocity| = angle/time = erp*theta / stepsize
- // = (erp*fps) * theta
- // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
- // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
- // ...as ax1 and ax2 are unit length. if theta is smallish,
- // theta ~= sin(theta), so
- // angular_velocity = (erp*fps) * (ax1 x ax2)
- // ax1 x ax2 is in the plane space of ax1, so we project the angular
- // velocity to p and q to find the right hand side.
- btVector3 ax2 = trB.getBasis().getColumn(2);
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the hinge back into alignment.
+ // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+ // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ btVector3 ax2 = trB.getBasis().getColumn(2);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
// check angular limits
- int nrow = 4; // last filled row
+ int nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
- if(getSolveLimit())
+ if (getSolveLimit())
{
-#ifdef _BT_USE_CENTER_LIMIT_
- limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+ limit_err = m_limit.getCorrection() * m_referenceSign;
#else
- limit_err = m_correction * m_referenceSign;
+ limit_err = m_correction * m_referenceSign;
#endif
- limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
- int powered = 0;
- if(getEnableAngularMotor())
- {
- powered = 1;
- }
- if(limit || powered)
+ bool powered = getEnableAngularMotor();
+ if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
- info->m_J1angularAxis[srow+0] = ax1[0];
- info->m_J1angularAxis[srow+1] = ax1[1];
- info->m_J1angularAxis[srow+2] = ax1[2];
+ info->m_J1angularAxis[srow + 0] = ax1[0];
+ info->m_J1angularAxis[srow + 1] = ax1[1];
+ info->m_J1angularAxis[srow + 2] = ax1[2];
- info->m_J2angularAxis[srow+0] = -ax1[0];
- info->m_J2angularAxis[srow+1] = -ax1[1];
- info->m_J2angularAxis[srow+2] = -ax1[2];
+ info->m_J2angularAxis[srow + 0] = -ax1[0];
+ info->m_J2angularAxis[srow + 1] = -ax1[1];
+ info->m_J2angularAxis[srow + 2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
- if(limit && (lostop == histop))
+ if (limit && (lostop == histop))
{ // the joint motor is ineffective
- powered = 0;
+ powered = false;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
- if(powered)
+ if (powered)
{
- if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+ if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
- info->m_lowerLimit[srow] = - m_maxMotorImpulse;
- info->m_upperLimit[srow] = m_maxMotorImpulse;
+ info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+ info->m_upperLimit[srow] = m_maxMotorImpulse;
}
- if(limit)
+ if (limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
- if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+ if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
- if(lostop == histop)
+ if (lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
- else if(limit == 1)
- { // low limit
+ else if (limit == 1)
+ { // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
- else
- { // high limit
+ else
+ { // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef _BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
- if(bounce > btScalar(0.0))
+ if (bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
- if(limit == 1)
- { // low limit
- if(vel < 0)
+ if (limit == 1)
+ { // low limit
+ if (vel < 0)
{
btScalar newc = -bounce * vel;
- if(newc > info->m_constraintError[srow])
+ if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
- { // high limit - all those computations are reversed
- if(vel > 0)
+ { // high limit - all those computations are reversed
+ if (vel > 0)
{
btScalar newc = -bounce * vel;
- if(newc < info->m_constraintError[srow])
+ if (newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
-#ifdef _BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
- } // if(limit)
- } // if angular limit or powered
+ } // if(limit)
+ } // if angular limit or powered
}
-
-void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_rbAFrame = frameA;
m_rbBFrame = frameB;
buildJacobian();
}
-
-void btHingeConstraint::updateRHS(btScalar timeStep)
+void btHingeConstraint::updateRHS(btScalar timeStep)
{
(void)timeStep;
-
}
-
-
-
btScalar btHingeConstraint::getHingeAngle()
{
- return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
}
-btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
+btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
{
- const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
- const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+ const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+ const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
-// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+ // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle;
}
-
-
-void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
+void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
{
// Compute limit information
- m_hingeAngle = getHingeAngle(transA,transB);
-#ifdef _BT_USE_CENTER_LIMIT_
+ m_hingeAngle = getHingeAngle(transA, transB);
+#ifdef _BT_USE_CENTER_LIMIT_
m_limit.test(m_hingeAngle);
#else
m_correction = btScalar(0.);
@@ -713,7 +672,7 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
- }
+ }
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
@@ -725,7 +684,6 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
return;
}
-
static btVector3 vHinge(0, 0, btScalar(1));
void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
@@ -735,14 +693,15 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
qConstraint.normalize();
// extract "pure" hinge component
- btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
+ btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
+ vNoHinge.normalize();
btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
qHinge.normalize();
// compute angular target, clamped to limits
btScalar targetAngle = qHinge.getAngle();
- if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = -(qHinge);
targetAngle = qHinge.getAngle();
@@ -755,7 +714,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
-#ifdef _BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
m_limit.fit(targetAngle);
#else
if (m_lowerLimit < m_upperLimit)
@@ -767,20 +726,18 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
}
#endif
// compute angular velocity
- btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
m_motorTargetVelocity = dAngle / dt;
}
-
-
-void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
+void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
- btTransform trA = transA*m_rbAFrame;
- btTransform trB = transB*m_rbBFrame;
+ btTransform trA = transA * m_rbAFrame;
+ btTransform trB = transB * m_rbBFrame;
// pivot point
// btVector3 pivotAInW = trA.getOrigin();
// btVector3 pivotBInW = trB.getOrigin();
@@ -793,11 +750,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
- if(miS > btScalar(0.f))
+ if (miS > btScalar(0.f))
{
factA = miB / miS;
}
- else
+ else
{
factA = btScalar(0.5f);
}
@@ -807,15 +764,21 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
btVector3 ax1A = trA.getBasis().getColumn(2);
btVector3 ax1B = trB.getBasis().getColumn(2);
btVector3 ax1 = ax1A * factA + ax1B * factB;
+ if (ax1.length2()<SIMD_EPSILON)
+ {
+ factA=0.f;
+ factB=1.f;
+ ax1 = ax1A * factA + ax1B * factB;
+ }
ax1.normalize();
- // fill first 3 rows
+ // fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
int s0 = 0;
int s1 = s;
int s2 = s * 2;
- int nrow = 2; // last filled row
+ int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
@@ -834,7 +797,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
- if(len2 > SIMD_EPSILON)
+ if (len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
@@ -847,44 +810,44 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
// fill three rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
- for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
- for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
+ for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i];
+ for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
- if(hasStaticBody && getSolveLimit())
- { // to make constraint between static and dynamic objects more rigid
+ if (hasStaticBody && getSolveLimit())
+ { // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
- for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
- for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
+ for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i];
+ for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i];
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
- if(hasStaticBody)
- { // to make constraint between static and dynamic objects more rigid
+ if (hasStaticBody)
+ { // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
- for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
- for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
+ for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i];
+ for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i];
- btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
+ btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
- for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
- for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
- for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
+ for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i];
+ for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i];
+ for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i];
- for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
- for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
- for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
+ for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i];
+ for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i];
+ for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i];
+
+ // compute three elements of right hand side
- // compute three elements of right hand side
-
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s0] = rhs;
rhs = k * q.dot(ofs);
@@ -929,150 +892,144 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
- k = info->fps * normalErp;//??
+ k = info->fps * normalErp; //??
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
#endif
// check angular limits
- nrow = 4; // last filled row
+ nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
- if(getSolveLimit())
+ if (getSolveLimit())
{
-#ifdef _BT_USE_CENTER_LIMIT_
- limit_err = m_limit.getCorrection() * m_referenceSign;
+#ifdef _BT_USE_CENTER_LIMIT_
+ limit_err = m_limit.getCorrection() * m_referenceSign;
#else
- limit_err = m_correction * m_referenceSign;
+ limit_err = m_correction * m_referenceSign;
#endif
- limit = (limit_err > btScalar(0.0)) ? 1 : 2;
-
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
- int powered = 0;
- if(getEnableAngularMotor())
- {
- powered = 1;
- }
- if(limit || powered)
+ bool powered = getEnableAngularMotor();
+ if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
- info->m_J1angularAxis[srow+0] = ax1[0];
- info->m_J1angularAxis[srow+1] = ax1[1];
- info->m_J1angularAxis[srow+2] = ax1[2];
+ info->m_J1angularAxis[srow + 0] = ax1[0];
+ info->m_J1angularAxis[srow + 1] = ax1[1];
+ info->m_J1angularAxis[srow + 2] = ax1[2];
- info->m_J2angularAxis[srow+0] = -ax1[0];
- info->m_J2angularAxis[srow+1] = -ax1[1];
- info->m_J2angularAxis[srow+2] = -ax1[2];
+ info->m_J2angularAxis[srow + 0] = -ax1[0];
+ info->m_J2angularAxis[srow + 1] = -ax1[1];
+ info->m_J2angularAxis[srow + 2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
- if(limit && (lostop == histop))
+ if (limit && (lostop == histop))
{ // the joint motor is ineffective
- powered = 0;
+ powered = false;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
- if(powered)
+ if (powered)
{
- if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
+ if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
- info->m_lowerLimit[srow] = - m_maxMotorImpulse;
- info->m_upperLimit[srow] = m_maxMotorImpulse;
+ info->m_lowerLimit[srow] = -m_maxMotorImpulse;
+ info->m_upperLimit[srow] = m_maxMotorImpulse;
}
- if(limit)
+ if (limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
- if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
+ if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
- if(lostop == histop)
+ if (lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
- else if(limit == 1)
- { // low limit
+ else if (limit == 1)
+ { // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
- else
- { // high limit
+ else
+ { // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
-#ifdef _BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
- if(bounce > btScalar(0.0))
+ if (bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
- if(limit == 1)
- { // low limit
- if(vel < 0)
+ if (limit == 1)
+ { // low limit
+ if (vel < 0)
{
btScalar newc = -bounce * vel;
- if(newc > info->m_constraintError[srow])
+ if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
- { // high limit - all those computations are reversed
- if(vel > 0)
+ { // high limit - all those computations are reversed
+ if (vel > 0)
{
btScalar newc = -bounce * vel;
- if(newc < info->m_constraintError[srow])
+ if (newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
-#ifdef _BT_USE_CENTER_LIMIT_
+#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
- } // if(limit)
- } // if angular limit or powered
+ } // if(limit)
+ } // if angular limit or powered
}
-
-///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btHingeConstraint::setParam(int num, btScalar value, int axis)
{
- if((axis == -1) || (axis == 5))
+ if ((axis == -1) || (axis == 5))
{
- switch(num)
- {
- case BT_CONSTRAINT_STOP_ERP :
+ switch (num)
+ {
+ case BT_CONSTRAINT_STOP_ERP:
m_stopERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_STOP;
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
m_stopCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_STOP;
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break;
@@ -1080,7 +1037,7 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
m_normalERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_NORM;
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
@@ -1091,22 +1048,22 @@ void btHingeConstraint::setParam(int num, btScalar value, int axis)
}
///return the local value of parameter
-btScalar btHingeConstraint::getParam(int num, int axis) const
+btScalar btHingeConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
- if((axis == -1) || (axis == 5))
+ if ((axis == -1) || (axis == 5))
{
- switch(num)
- {
- case BT_CONSTRAINT_STOP_ERP :
+ switch (num)
+ {
+ case BT_CONSTRAINT_STOP_ERP:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
retVal = m_stopERP;
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
retVal = m_stopCFM;
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM;
break;
@@ -1114,7 +1071,7 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
retVal = m_normalERP;
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
@@ -1124,5 +1081,3 @@ btScalar btHingeConstraint::getParam(int num, int axis) const
}
return retVal;
}
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-14 03:41:40 +0300