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/btFixedConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp')
-rw-r--r--extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp149
1 files changed, 103 insertions, 46 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
index f93a3280f35..3428e0b069d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
@@ -23,9 +23,8 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
{
- m_pivotInA = frameInA.getOrigin();
- m_pivotInB = frameInB.getOrigin();
- m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+ m_frameInA = frameInA;
+ m_frameInB = frameInB;
}
@@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint ()
void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 6;
- info->nub = 6;
+ info->nub = 0;
}
void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
-
+ const btTransform& transA = m_rbA.getCenterOfMassTransform();
+ const btTransform& transB = m_rbB.getCenterOfMassTransform();
+
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
@@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
- btVector3 a1 = worldOrnA*m_pivotInA;
- {
+ btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
+ {
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
-
+
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
@@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
- btVector3 a2 = worldOrnB*m_pivotInB;
-
- {
- // btVector3 a2n = -a2;
+ btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
+ {
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
@@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
int j;
for (j=0; j<3; j++)
{
-
-
-
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
- //fix the 3 angular degrees of freedom
-
- int start_row = 3;
- int s = info->rowskip;
- int start_index = start_row * s;
-
- // 3 rows to make body rotations equal
- info->m_J1angularAxis[start_index] = 1;
- info->m_J1angularAxis[start_index + s + 1] = 1;
- info->m_J1angularAxis[start_index + s*2+2] = 1;
- if ( info->m_J2angularAxis)
- {
- info->m_J2angularAxis[start_index] = -1;
- info->m_J2angularAxis[start_index + s+1] = -1;
- info->m_J2angularAxis[start_index + s*2+2] = -1;
- }
-
- // set right hand side for the angular dofs
-
- btVector3 diff;
- btScalar angle;
- btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
- btQuaternion qrelCur;
- mrelCur.getRotation(qrelCur);
- btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
- diff*=-angle;
- for (j=0; j<3; j++)
- {
- info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
- }
-
+ btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
+ btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
+ btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
+ btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
+ btVector3 target;
+ btScalar x = ivB.dot(ivA);
+ btScalar y = ivB.dot(jvA);
+ btScalar z = ivB.dot(kvA);
+ btVector3 swingAxis(0,0,0);
+ {
+ if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+ {
+ swingAxis = -ivB.cross(ivA);
+ }
+ }
+ btVector3 vTwist(1,0,0);
+
+ // compute rotation of A wrt B (in constraint space)
+ btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
+ btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
+ btQuaternion qAB = qB.inverse() * qA;
+ // split rotation into cone and twist
+ // (all this is done from B's perspective. Maybe I should be averaging axes...)
+ btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+ btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+ btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+ int row = 3;
+ int srow = row * info->rowskip;
+ btVector3 ax1;
+ // angular limits
+ {
+ btScalar *J1 = info->m_J1angularAxis;
+ btScalar *J2 = info->m_J2angularAxis;
+ btTransform trA = transA*m_frameInA;
+ btVector3 twistAxis = trA.getBasis().getColumn(0);
+
+ btVector3 p = trA.getBasis().getColumn(1);
+ btVector3 q = trA.getBasis().getColumn(2);
+ int srow1 = srow + info->rowskip;
+ J1[srow+0] = p[0];
+ J1[srow+1] = p[1];
+ J1[srow+2] = p[2];
+ J1[srow1+0] = q[0];
+ J1[srow1+1] = q[1];
+ J1[srow1+2] = q[2];
+ J2[srow+0] = -p[0];
+ J2[srow+1] = -p[1];
+ J2[srow+2] = -p[2];
+ J2[srow1+0] = -q[0];
+ J2[srow1+1] = -q[1];
+ J2[srow1+2] = -q[2];
+ btScalar fact = info->fps;
+ info->m_constraintError[srow] = fact * swingAxis.dot(p);
+ info->m_constraintError[srow1] = fact * swingAxis.dot(q);
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+ info->m_upperLimit[srow1] = SIMD_INFINITY;
+ srow = srow1 + info->rowskip;
+
+ {
+ btQuaternion qMinTwist = qABTwist;
+ btScalar twistAngle = qABTwist.getAngle();
+
+ if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ {
+ qMinTwist = -(qABTwist);
+ twistAngle = qMinTwist.getAngle();
+ }
+
+ if (twistAngle > SIMD_EPSILON)
+ {
+ twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+ twistAxis.normalize();
+ twistAxis = quatRotate(qB, -twistAxis);
+ }
+ ax1 = twistAxis;
+ btScalar *J1 = info->m_J1angularAxis;
+ btScalar *J2 = info->m_J2angularAxis;
+ 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];
+ btScalar k = info->fps;
+ info->m_constraintError[srow] = k * twistAngle;
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ }
} \ No newline at end of file
generated by cgit v1.2.3 (git 2.25.1) at 2024-08-10 08:02:37 +0300