summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp640
1 files changed, 345 insertions, 295 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 540dcd18f7..49c8d9bbf7 100644
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -37,67 +37,54 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
-
-
#include "btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include <new>
-
-
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder)
- : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB)
- , m_rotateOrder(rotOrder)
- , m_flags(0)
+ : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
{
calculateTransforms();
}
-
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder)
- : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
- , m_frameInB(frameInB)
- , m_rotateOrder(rotOrder)
- , m_flags(0)
+ : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
{
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
calculateTransforms();
}
-
btScalar btGeneric6DofSpring2Constraint::btGetMatrixElem(const btMatrix3x3& mat, int index)
{
- int i = index%3;
- int j = index/3;
+ int i = index % 3;
+ int j = index / 3;
return mat[i][j];
}
// MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cy*cz -cy*sz sy
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
- btScalar fi = btGetMatrixElem(mat,2);
+ btScalar fi = btGetMatrixElem(mat, 2);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
- xyz[1] = btAsin(btGetMatrixElem(mat,2));
- xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
+ xyz[1] = btAsin(btGetMatrixElem(mat, 2));
+ xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
- xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = -SIMD_HALF_PI;
xyz[2] = btScalar(0.0);
return false;
@@ -106,32 +93,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXYZ(const btMatrix3x3& mat,btV
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
- xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
xyz[1] = SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
-bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cy*cz -sz sy*cz
// cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
// cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
- btScalar fi = btGetMatrixElem(mat,1);
+ btScalar fi = btGetMatrixElem(mat, 1);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
- xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
- xyz[2] = btAsin(-btGetMatrixElem(mat,1));
+ xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
+ xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
+ xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
return true;
}
else
{
- xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+ xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
xyz[1] = btScalar(0.0);
xyz[2] = SIMD_HALF_PI;
return false;
@@ -139,33 +126,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btV
}
else
{
- xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
+ xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
xyz[1] = 0.0;
xyz[2] = -SIMD_HALF_PI;
}
return false;
}
-bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
// cx*sz cx*cz -sx
// cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
- btScalar fi = btGetMatrixElem(mat,5);
+ btScalar fi = btGetMatrixElem(mat, 5);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAsin(-btGetMatrixElem(mat,5));
- xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
- xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
+ xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
+ xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
return true;
}
else
{
xyz[0] = SIMD_HALF_PI;
- xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
xyz[2] = btScalar(0.0);
return false;
}
@@ -173,32 +160,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btV
else
{
xyz[0] = -SIMD_HALF_PI;
- xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
xyz[2] = 0.0;
}
return false;
}
-bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
- btScalar fi = btGetMatrixElem(mat,3);
+ btScalar fi = btGetMatrixElem(mat, 3);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
- xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
- xyz[2] = btAsin(btGetMatrixElem(mat,3));
+ xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
+ xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
+ xyz[2] = btAsin(btGetMatrixElem(mat, 3));
return true;
}
else
{
xyz[0] = btScalar(0.0);
- xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+ xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
xyz[2] = -SIMD_HALF_PI;
return false;
}
@@ -206,33 +193,33 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btV
else
{
xyz[0] = btScalar(0.0);
- xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
+ xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
xyz[2] = SIMD_HALF_PI;
}
return false;
}
-bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
// cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
// -cx*sy sx cx*cy
- btScalar fi = btGetMatrixElem(mat,7);
+ btScalar fi = btGetMatrixElem(mat, 7);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAsin(btGetMatrixElem(mat,7));
- xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
- xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
+ xyz[0] = btAsin(btGetMatrixElem(mat, 7));
+ xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
+ xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
return true;
}
else
{
xyz[0] = -SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
- xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+ xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
return false;
}
}
@@ -240,32 +227,32 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btV
{
xyz[0] = SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
- xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
+ xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
}
return false;
}
-bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
+bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat, btVector3& xyz)
{
// rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
// cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
// -sy cy*sx cy*cx
- btScalar fi = btGetMatrixElem(mat,6);
+ btScalar fi = btGetMatrixElem(mat, 6);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
- xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
- xyz[1] = btAsin(-btGetMatrixElem(mat,6));
- xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
+ xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
+ xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
+ xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
return true;
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = SIMD_HALF_PI;
- xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
+ xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
return false;
}
}
@@ -273,23 +260,36 @@ bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btV
{
xyz[0] = btScalar(0.0);
xyz[1] = -SIMD_HALF_PI;
- xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
+ xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
}
return false;
}
void btGeneric6DofSpring2Constraint::calculateAngleInfo()
{
- btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+ btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
switch (m_rotateOrder)
{
- case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
- case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
- case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
- case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
- case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
- case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
- default : btAssert(false);
+ case RO_XYZ:
+ matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ case RO_XZY:
+ matrixToEulerXZY(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ case RO_YXZ:
+ matrixToEulerYXZ(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ case RO_YZX:
+ matrixToEulerYZX(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ case RO_ZXY:
+ matrixToEulerZXY(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ case RO_ZYX:
+ matrixToEulerZYX(relative_frame, m_calculatedAxisAngleDiff);
+ break;
+ default:
+ btAssert(false);
}
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
@@ -307,14 +307,14 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
// to the components of w and set that to 0.
switch (m_rotateOrder)
{
- case RO_XYZ :
+ case RO_XYZ:
{
//Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
//The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
//Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
//that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
// x' = Nperp = N.cross(axis2)
- // y' = N = axis2.cross(axis0)
+ // y' = N = axis2.cross(axis0)
// z' = z
//
// x" = X
@@ -324,7 +324,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
//first rotate around z
//second rotate around y'= z.cross(X)
//third rotate around x" = X
- //Original XYZ extrinsic rotation order.
+ //Original XYZ extrinsic rotation order.
//Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X)
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -333,7 +333,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
break;
}
- case RO_XZY :
+ case RO_XZY:
{
//planes: xz,ZY normals: y, X
//first rotate around y
@@ -346,7 +346,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
break;
}
- case RO_YXZ :
+ case RO_YXZ:
{
//planes: yx,XZ normals: z, Y
//first rotate around z
@@ -359,7 +359,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
break;
}
- case RO_YZX :
+ case RO_YZX:
{
//planes: yz,ZX normals: x, Y
//first rotate around x
@@ -372,7 +372,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
break;
}
- case RO_ZXY :
+ case RO_ZXY:
{
//planes: zx,XY normals: y, Z
//first rotate around y
@@ -385,7 +385,7 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
break;
}
- case RO_ZYX :
+ case RO_ZYX:
{
//planes: zy,YX normals: x, Z
//first rotate around x
@@ -398,22 +398,21 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo()
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
break;
}
- default:
- btAssert(false);
+ default:
+ btAssert(false);
}
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
-
}
void btGeneric6DofSpring2Constraint::calculateTransforms()
{
- calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
}
-void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
+void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
{
m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = transB * m_frameInB;
@@ -424,18 +423,17 @@ void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& tran
btScalar miB = getRigidBodyB().getInvMass();
m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
- if(miS > btScalar(0.f))
+ if (miS > btScalar(0.f))
{
m_factA = miB / miS;
}
- else
+ else
{
m_factA = btScalar(0.5f);
}
m_factB = btScalar(1.0f) - m_factA;
}
-
void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
{
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
@@ -444,35 +442,37 @@ void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
m_angularLimits[axis_index].testLimitValue(angle);
}
-
-void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info)
+void btGeneric6DofSpring2Constraint::getInfo1(btConstraintInfo1* info)
{
//prepare constraint
- calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+ calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
info->m_numConstraintRows = 0;
info->nub = 0;
int i;
//test linear limits
- for(i = 0; i < 3; i++)
+ for (i = 0; i < 3; i++)
{
- if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
- else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
- if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1;
+ if (m_linearLimits.m_currentLimit[i] == 4)
+ info->m_numConstraintRows += 2;
+ else if (m_linearLimits.m_currentLimit[i] != 0)
+ info->m_numConstraintRows += 1;
+ if (m_linearLimits.m_enableMotor[i]) info->m_numConstraintRows += 1;
if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1;
}
//test angular limits
- for (i=0;i<3 ;i++ )
+ for (i = 0; i < 3; i++)
{
testAngularLimitMotor(i);
- if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
- else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
- if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
+ if (m_angularLimits[i].m_currentLimit == 4)
+ info->m_numConstraintRows += 2;
+ else if (m_angularLimits[i].m_currentLimit != 0)
+ info->m_numConstraintRows += 1;
+ if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
}
}
-
-void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
+void btGeneric6DofSpring2Constraint::getInfo2(btConstraintInfo2* info)
{
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
@@ -482,118 +482,138 @@ void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info)
const btVector3& angVelB = m_rbB.getAngularVelocity();
// for stability better to solve angular limits first
- int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
- setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
+ int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
+ setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
-
-int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
{
//solve linear limits
btRotationalLimitMotor2 limot;
- for (int i=0;i<3 ;i++ )
+ for (int i = 0; i < 3; i++)
{
- if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
- { // re-use rotational motor code
- limot.m_bounce = m_linearLimits.m_bounce[i];
- limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
- limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
- limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
- limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
- limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
- limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
- limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
- limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
- limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
+ if (m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
+ { // re-use rotational motor code
+ limot.m_bounce = m_linearLimits.m_bounce[i];
+ limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+ limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
+ limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+ limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
+ limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+ limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
+ limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
+ limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
+ limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
- limot.m_springDamping = m_linearLimits.m_springDamping[i];
- limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
- limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
- limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
- limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
- limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
- limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
+ limot.m_springDamping = m_linearLimits.m_springDamping[i];
+ limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
+ limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
+ limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+ limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+ limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+ limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
- limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
- limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
+ limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
+ limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
- int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
- #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
+ int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
+#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
- m_angularLimits[indx1].m_currentLimit == 2 ||
- ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
- ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
+ m_angularLimits[indx1].m_currentLimit == 2 ||
+ (m_angularLimits[indx1].m_currentLimit == 3 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
+ (m_angularLimits[indx1].m_currentLimit == 4 && (m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
- m_angularLimits[indx2].m_currentLimit == 2 ||
- ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
- ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
- if( indx1Violated && indx2Violated )
+ m_angularLimits[indx2].m_currentLimit == 2 ||
+ (m_angularLimits[indx2].m_currentLimit == 3 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION)) ||
+ (m_angularLimits[indx2].m_currentLimit == 4 && (m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION));
+ if (indx1Violated && indx2Violated)
{
rotAllowed = 0;
}
- row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
-
+ row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
}
}
return row;
}
-
-
-int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
+int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
{
int row = row_offset;
//order of rotational constraint rows
int cIdx[] = {0, 1, 2};
- switch(m_rotateOrder)
+ switch (m_rotateOrder)
{
- case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
- case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
- case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
- case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
- case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
- case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
- default : btAssert(false);
+ case RO_XYZ:
+ cIdx[0] = 0;
+ cIdx[1] = 1;
+ cIdx[2] = 2;
+ break;
+ case RO_XZY:
+ cIdx[0] = 0;
+ cIdx[1] = 2;
+ cIdx[2] = 1;
+ break;
+ case RO_YXZ:
+ cIdx[0] = 1;
+ cIdx[1] = 0;
+ cIdx[2] = 2;
+ break;
+ case RO_YZX:
+ cIdx[0] = 1;
+ cIdx[1] = 2;
+ cIdx[2] = 0;
+ break;
+ case RO_ZXY:
+ cIdx[0] = 2;
+ cIdx[1] = 0;
+ cIdx[2] = 1;
+ break;
+ case RO_ZYX:
+ cIdx[0] = 2;
+ cIdx[1] = 1;
+ cIdx[2] = 0;
+ break;
+ default:
+ btAssert(false);
}
- for (int ii = 0; ii < 3 ; ii++ )
+ for (int ii = 0; ii < 3; ii++)
{
int i = cIdx[ii];
- if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
+ if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
{
btVector3 axis = getAxis(i);
int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
- if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
+ if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
- if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
+ if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
{
m_angularLimits[i].m_stopERP = info->erp;
}
- if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
+ if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
{
m_angularLimits[i].m_motorCFM = info->cfm[0];
}
- if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
+ if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
{
m_angularLimits[i].m_motorERP = info->erp;
}
- row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
+ row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
}
}
return row;
}
-
void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
{
m_frameInA = frameA;
@@ -602,32 +622,31 @@ void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const
calculateTransforms();
}
-
void btGeneric6DofSpring2Constraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
- for(int i = 0; i < 3; i++)
+ for (int i = 0; i < 3; i++)
{
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
}
}
-void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
+void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
{
- btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
- btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
+ btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+ btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
- J1[srow+0] = ax1[0];
- J1[srow+1] = ax1[1];
- J1[srow+2] = ax1[2];
+ J1[srow + 0] = ax1[0];
+ J1[srow + 1] = ax1[1];
+ J1[srow + 2] = ax1[2];
- J2[srow+0] = -ax1[0];
- J2[srow+1] = -ax1[1];
- J2[srow+2] = -ax1[2];
+ J2[srow + 0] = -ax1[0];
+ J2[srow + 1] = -ax1[1];
+ J2[srow + 2] = -ax1[2];
- if(!rotational)
+ if (!rotational)
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
@@ -636,40 +655,44 @@ void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * l
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
- if(m_hasStaticBody && (!rotAllowed))
+ if (m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
int i;
- for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
- for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
+ for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
+ for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
}
}
-
int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
- btRotationalLimitMotor2 * limot,
- const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
- btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
+ btRotationalLimitMotor2* limot,
+ const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
+ btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
{
int count = 0;
int srow = row * info->rowskip;
- if (limot->m_currentLimit==4)
+ if (limot->m_currentLimit == 4)
{
btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
- if (rotational) {
- if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
- btScalar bounceerror = -limot->m_bounce* vel;
+ if (rotational)
+ {
+ if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
+ {
+ btScalar bounceerror = -limot->m_bounce * vel;
if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
}
- } else {
- if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
- btScalar bounceerror = -limot->m_bounce* vel;
+ }
+ else
+ {
+ if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
+ {
+ btScalar bounceerror = -limot->m_bounce * vel;
if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
}
}
@@ -679,16 +702,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
srow += info->rowskip;
++count;
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
- if (rotational) {
- if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
- btScalar bounceerror = -limot->m_bounce* vel;
+ if (rotational)
+ {
+ if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
+ {
+ btScalar bounceerror = -limot->m_bounce * vel;
if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
}
- } else {
- if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
- btScalar bounceerror = -limot->m_bounce* vel;
+ }
+ else
+ {
+ if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
+ {
+ btScalar bounceerror = -limot->m_bounce * vel;
if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
}
}
@@ -697,10 +725,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
info->cfm[srow] = limot->m_stopCFM;
srow += info->rowskip;
++count;
- } else
- if (limot->m_currentLimit==3)
+ }
+ else if (limot->m_currentLimit == 3)
{
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
@@ -711,13 +739,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (limot->m_enableMotor && !limot->m_servoMotor)
{
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
- btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
- limot->m_loLimit,
- limot->m_hiLimit,
- tag_vel,
- info->fps * limot->m_motorERP);
+ btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
+ limot->m_loLimit,
+ limot->m_hiLimit,
+ tag_vel,
+ info->fps * limot->m_motorERP);
info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
@@ -735,36 +763,36 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (error > SIMD_PI)
{
error -= SIMD_2_PI;
- curServoTarget +=SIMD_2_PI;
+ curServoTarget += SIMD_2_PI;
}
if (error < -SIMD_PI)
{
error += SIMD_2_PI;
- curServoTarget -=SIMD_2_PI;
+ curServoTarget -= SIMD_2_PI;
}
}
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
- btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
+ btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
btScalar tag_vel = -targetvelocity;
btScalar mot_fact;
- if(error != 0)
+ if (error != 0)
{
btScalar lowLimit;
btScalar hiLimit;
- if(limot->m_loLimit > limot->m_hiLimit)
+ if (limot->m_loLimit > limot->m_hiLimit)
{
lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
- hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
+ hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
}
else
{
- lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
- hiLimit = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
+ lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
+ hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
}
mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
- }
- else
+ }
+ else
{
mot_fact = 0;
}
@@ -779,7 +807,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (limot->m_enableSpring)
{
btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
- calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
+ calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
//btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
//if(cfm > 0.99999)
@@ -792,34 +820,46 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar dt = BT_ONE / info->fps;
btScalar kd = limot->m_springDamping;
btScalar ks = limot->m_springStiffness;
- btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
-// btScalar erp = 0.1;
+ btScalar vel;
+ if (rotational)
+ {
+ vel = angVelA.dot(ax1) - angVelB.dot(ax1);
+ }
+ else
+ {
+ btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
+ btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
+ vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
+ }
btScalar cfm = BT_ZERO;
btScalar mA = BT_ONE / m_rbA.getInvMass();
btScalar mB = BT_ONE / m_rbB.getInvMass();
- if (rotational) {
+ if (rotational)
+ {
btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
}
- btScalar m = mA > mB ? mB : mA;
+ btScalar m;
+ if (m_rbA.getInvMass() == 0) m = mB; else
+ if (m_rbB.getInvMass() == 0) m = mA; else
+ m = mA*mB / (mA + mB);
btScalar angularfreq = sqrt(ks / m);
-
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
- if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
+ if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
{
ks = BT_ONE / dt / dt / btScalar(16.0) * m;
}
//avoid damping that would blow up the spring
- if(limot->m_springDampingLimited && kd * dt > m)
+ if (limot->m_springDampingLimited && kd * dt > m)
{
kd = m / dt;
}
btScalar fs = ks * error * dt;
btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
- btScalar f = (fs+fd);
+ btScalar f = (fs + fd);
// after the spring force affecting the body(es) the new velocity will be
// vel + f / m * (rotational ? -1 : 1)
@@ -828,15 +868,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
// however in practice any value is fine as long as it is greater then the "proper" velocity,
// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+ // (Even with our best intent the "new" velocity is only an estimation. If we underestimate
+ // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
+ // matter, because the solver will limit it according the force limit)
// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
// will we not request a velocity with the wrong direction ?
- // and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+ // and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
// so the sign of the force that is really matters
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
btScalar minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f;
- if(!rotational)
+ if (!rotational)
{
info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
@@ -855,56 +898,55 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
return count;
}
-
-//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 btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
{
- if((axis >= 0) && (axis < 3))
+ if ((axis >= 0) && (axis < 3))
{
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_STOP_ERP :
+ case BT_CONSTRAINT_STOP_ERP:
m_linearLimits.m_stopERP[axis] = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_ERP :
+ case BT_CONSTRAINT_ERP:
m_linearLimits.m_motorERP[axis] = value;
m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
m_linearLimits.m_motorCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
- else if((axis >=3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_STOP_ERP :
+ case BT_CONSTRAINT_STOP_ERP:
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_ERP :
+ case BT_CONSTRAINT_ERP:
m_angularLimits[axis - 3].m_motorERP = value;
m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
m_angularLimits[axis - 3].m_motorCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2);
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
@@ -915,54 +957,54 @@ void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
}
//return the local value of parameter
-btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
+btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
- if((axis >= 0) && (axis < 3))
+ if ((axis >= 0) && (axis < 3))
{
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_STOP_ERP :
+ case BT_CONSTRAINT_STOP_ERP:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_linearLimits.m_stopERP[axis];
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
- case BT_CONSTRAINT_ERP :
+ case BT_CONSTRAINT_ERP:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_linearLimits.m_motorERP[axis];
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_linearLimits.m_motorCFM[axis];
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
- else if((axis >=3) && (axis < 6))
+ else if ((axis >= 3) && (axis < 6))
{
- switch(num)
+ switch (num)
{
- case BT_CONSTRAINT_STOP_ERP :
+ case BT_CONSTRAINT_STOP_ERP:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
- case BT_CONSTRAINT_STOP_CFM :
+ case BT_CONSTRAINT_STOP_CFM:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
- case BT_CONSTRAINT_ERP :
+ case BT_CONSTRAINT_ERP:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_angularLimits[axis - 3].m_motorERP;
break;
- case BT_CONSTRAINT_CFM :
+ case BT_CONSTRAINT_CFM:
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
retVal = m_angularLimits[axis - 3].m_motorCFM;
break;
- default :
+ default:
btAssertConstrParams(0);
}
}
@@ -973,31 +1015,29 @@ btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
return retVal;
}
-
-
-void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2)
+void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
btVector3 zAxis = axis1.normalized();
btVector3 yAxis = axis2.normalized();
- btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
-
+ btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
+
btTransform frameInW;
frameInW.setIdentity();
- frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
- xAxis[1], yAxis[1], zAxis[1],
- xAxis[2], yAxis[2], zAxis[2]);
-
+ frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
+ xAxis[1], yAxis[1], zAxis[1],
+ xAxis[2], yAxis[2], zAxis[2]);
+
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
-
+
calculateTransforms();
}
void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_bounce[index] = bounce;
else
m_angularLimits[index - 3].m_bounce = bounce;
@@ -1006,7 +1046,7 @@ void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce)
void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_enableMotor[index] = onOff;
else
m_angularLimits[index - 3].m_enableMotor = onOff;
@@ -1015,7 +1055,7 @@ void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff)
void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_servoMotor[index] = onOff;
else
m_angularLimits[index - 3].m_servoMotor = onOff;
@@ -1024,18 +1064,16 @@ void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_targetVelocity[index] = velocity;
else
m_angularLimits[index - 3].m_targetVelocity = velocity;
}
-
-
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
{
m_linearLimits.m_servoTarget[index] = targetOrg;
}
@@ -1044,23 +1082,24 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr
//wrap between -PI and PI, see also
//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
- btScalar target = targetOrg+SIMD_PI;
+ btScalar target = targetOrg + SIMD_PI;
if (1)
{
- btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
+ btScalar m = target - SIMD_2_PI * floor(target / SIMD_2_PI);
// handle boundary cases resulted from floating-point cut off:
{
- if (m>=SIMD_2_PI)
+ if (m >= SIMD_2_PI)
{
target = 0;
- } else
+ }
+ else
{
- if (m<0 )
+ if (m < 0)
{
- if (SIMD_2_PI+m == SIMD_2_PI)
+ if (SIMD_2_PI + m == SIMD_2_PI)
target = 0;
else
- target = SIMD_2_PI+m;
+ target = SIMD_2_PI + m;
}
else
{
@@ -1070,7 +1109,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr
}
target -= SIMD_PI;
}
-
+
m_angularLimits[index - 3].m_servoTarget = target;
}
}
@@ -1078,7 +1117,7 @@ void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOr
void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_maxMotorForce[index] = force;
else
m_angularLimits[index - 3].m_maxMotorForce = force;
@@ -1087,19 +1126,22 @@ void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)
void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_enableSpring[index] = onOff;
else
- m_angularLimits[index - 3] .m_enableSpring = onOff;
+ m_angularLimits[index - 3].m_enableSpring = onOff;
}
void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
{
btAssert((index >= 0) && (index < 6));
- if (index<3) {
+ if (index < 3)
+ {
m_linearLimits.m_springStiffness[index] = stiffness;
m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
- } else {
+ }
+ else
+ {
m_angularLimits[index - 3].m_springStiffness = stiffness;
m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
}
@@ -1108,10 +1150,13 @@ void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness,
void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
{
btAssert((index >= 0) && (index < 6));
- if (index<3) {
+ if (index < 3)
+ {
m_linearLimits.m_springDamping[index] = damping;
m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
- } else {
+ }
+ else
+ {
m_angularLimits[index - 3].m_springDamping = damping;
m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
}
@@ -1121,9 +1166,9 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint()
{
calculateTransforms();
int i;
- for( i = 0; i < 3; i++)
+ for (i = 0; i < 3; i++)
m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
- for(i = 0; i < 3; i++)
+ for (i = 0; i < 3; i++)
m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
}
@@ -1131,35 +1176,38 @@ void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index)
{
btAssert((index >= 0) && (index < 6));
calculateTransforms();
- if (index<3)
+ if (index < 3)
m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
else
- m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
+ m_angularLimits[index - 3].m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
}
void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val)
{
btAssert((index >= 0) && (index < 6));
- if (index<3)
+ if (index < 3)
m_linearLimits.m_equilibriumPoint[index] = val;
else
- m_angularLimits[index - 3] .m_equilibriumPoint = val;
+ m_angularLimits[index - 3].m_equilibriumPoint = val;
}
-
//////////////////////////// btRotationalLimitMotor2 ////////////////////////////////////
void btRotationalLimitMotor2::testLimitValue(btScalar test_value)
{
//we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
- if(m_loLimit > m_hiLimit) {
+ if (m_loLimit > m_hiLimit)
+ {
m_currentLimit = 0;
m_currentLimitError = btScalar(0.f);
}
- else if(m_loLimit == m_hiLimit) {
+ else if (m_loLimit == m_hiLimit)
+ {
m_currentLimitError = test_value - m_loLimit;
m_currentLimit = 3;
- } else {
+ }
+ else
+ {
m_currentLimitError = test_value - m_loLimit;
m_currentLimitErrorHi = test_value - m_hiLimit;
m_currentLimit = 4;
@@ -1172,18 +1220,20 @@ void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_va
{
btScalar loLimit = m_lowerLimit[limitIndex];
btScalar hiLimit = m_upperLimit[limitIndex];
- if(loLimit > hiLimit) {
+ if (loLimit > hiLimit)
+ {
m_currentLimitError[limitIndex] = 0;
m_currentLimit[limitIndex] = 0;
}
- else if(loLimit == hiLimit) {
+ else if (loLimit == hiLimit)
+ {
m_currentLimitError[limitIndex] = test_value - loLimit;
m_currentLimit[limitIndex] = 3;
- } else {
+ }
+ else
+ {
m_currentLimitError[limitIndex] = test_value - loLimit;
m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
m_currentLimit[limitIndex] = 4;
}
}
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 09:46:26 +0000