From e80feca36b63cb88fedf1ceef97a94a6b939bad7 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Mon, 1 Dec 2008 06:41:25 +0000 Subject: [PATCH] Big work-in-progress refactoring of the constraint solver: 1) Add fast branchless SIMD support for constraint solver (Windows only until we get other contributions). See resolveSingleConstraintRowGenericSIMD in Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp resolveSingleConstraintRowGenericSIMD can be used for all constraints, including contact, point 2 point, hinge, generic etc. 2) During this refactoring, all constraints support the obsolete 'solveConstraintObsolete' while we add 'getInfo1' and 'getInfo2' support. This interface is almost identical interface to Open Dynamics Engine, to make it easier to port Dantzig LCP solver. 3) Some minor refactoring to reduce huge constructor overhead in math classes. --- Demos/MultiThreadedDemo/MultiThreadedDemo.cpp | 6 +- Demos/OpenGL/DemoApplication.h | 1 + .../btOdeQuickstepConstraintSolver.cpp | 4 +- Extras/quickstep/btOdeTypedJoint.cpp | 36 +- .../CollisionShapes/btBoxShape.h | 18 +- .../btConeTwistConstraint.cpp | 403 +++++---- .../ConstraintSolver/btConeTwistConstraint.h | 8 +- .../ConstraintSolver/btContactSolverInfo.h | 6 +- .../btGeneric6DofConstraint.cpp | 847 ++++++++++++------ .../btGeneric6DofConstraint.h | 19 +- .../ConstraintSolver/btHingeConstraint.cpp | 426 +++++---- .../ConstraintSolver/btHingeConstraint.h | 7 +- .../btPoint2PointConstraint.cpp | 213 +++-- .../btPoint2PointConstraint.h | 9 +- .../btSequentialImpulseConstraintSolver.cpp | 695 ++++++++------ .../btSequentialImpulseConstraintSolver.h | 26 +- .../ConstraintSolver/btSliderConstraint.cpp | 99 +- .../ConstraintSolver/btSliderConstraint.h | 10 +- .../ConstraintSolver/btSolverBody.h | 236 +++-- .../ConstraintSolver/btSolverConstraint.h | 36 +- .../ConstraintSolver/btTypedConstraint.h | 48 +- .../SpuSolverTask/SpuParallellSolverTask.h | 20 +- src/LinearMath/btQuadWord.h | 18 +- src/LinearMath/btQuaternion.h | 1 + src/LinearMath/btVector3.h | 222 ++++- 25 files changed, 2099 insertions(+), 1315 deletions(-) diff --git a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp index 6c633e416..62b2cd10a 100644 --- a/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp +++ b/Demos/MultiThreadedDemo/MultiThreadedDemo.cpp @@ -102,7 +102,7 @@ void MultiThreadedDemo::createStack( btCollisionShape* boxShape, float halfCubeS int rowSize = size - i; for(int j=0; j< rowSize; j++) { - btVector4 pos; + btVector3 pos; pos.setValue( -rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize, halfCubeSize + i * halfCubeSize * 2.0f, @@ -148,9 +148,10 @@ void MultiThreadedDemo::clientMoveAndDisplay() if (m_dynamicsWorld) { -//#define FIXED_STEP 1 +#define FIXED_STEP 1 #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(1.0f/60.f,0); + //CProfileManager::dumpAll(); #else //during idle mode, just run 1 simulation step maximum @@ -354,6 +355,7 @@ m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32Threa m_dynamicsWorld = world; world->getSolverInfo().m_numIterations = 4; + world->getSolverInfo().m_solverMode = SOLVER_SIMD+SOLVER_USE_WARMSTARTING; m_dynamicsWorld->getDispatchInfo().m_enableSPU = true; m_dynamicsWorld->setGravity(btVector3(0,-10,0)); diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h index 881daa680..c43711c8b 100644 --- a/Demos/OpenGL/DemoApplication.h +++ b/Demos/OpenGL/DemoApplication.h @@ -102,6 +102,7 @@ public: } + void setOrthographicProjection(); void resetPerspectiveProjection(); diff --git a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp index b0489195c..9c461f5af 100644 --- a/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp +++ b/Extras/quickstep/btOdeQuickstepConstraintSolver.cpp @@ -206,8 +206,8 @@ int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedOb body->m_originalBody = orgBody; - body->m_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); + body->m_facc.setValue(0,0,0); + body->m_tacc.setValue(0,0,0); body->m_linearVelocity = orgBody->getLinearVelocity(); body->m_angularVelocity = orgBody->getAngularVelocity(); diff --git a/Extras/quickstep/btOdeTypedJoint.cpp b/Extras/quickstep/btOdeTypedJoint.cpp index c289bed6f..74dc003eb 100644 --- a/Extras/quickstep/btOdeTypedJoint.cpp +++ b/Extras/quickstep/btOdeTypedJoint.cpp @@ -96,16 +96,6 @@ void OdeP2PJoint::GetInfo2(Info2 *info) { body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); } -// btScalar body0_mat[12]; -// body0_mat[0] = body0_trans.getBasis()[0][0]; -// body0_mat[1] = body0_trans.getBasis()[0][1]; -// body0_mat[2] = body0_trans.getBasis()[0][2]; -// body0_mat[4] = body0_trans.getBasis()[1][0]; -// body0_mat[5] = body0_trans.getBasis()[1][1]; -// body0_mat[6] = body0_trans.getBasis()[1][2]; -// body0_mat[8] = body0_trans.getBasis()[2][0]; -// body0_mat[9] = body0_trans.getBasis()[2][1]; -// body0_mat[10] = body0_trans.getBasis()[2][2]; btTransform body1_trans; @@ -113,23 +103,8 @@ void OdeP2PJoint::GetInfo2(Info2 *info) { body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); } -// btScalar body1_mat[12]; -// body1_mat[0] = body1_trans.getBasis()[0][0]; -// body1_mat[1] = body1_trans.getBasis()[0][1]; -// body1_mat[2] = body1_trans.getBasis()[0][2]; -// body1_mat[4] = body1_trans.getBasis()[1][0]; -// body1_mat[5] = body1_trans.getBasis()[1][1]; -// body1_mat[6] = body1_trans.getBasis()[1][2]; -// body1_mat[8] = body1_trans.getBasis()[2][0]; -// body1_mat[9] = body1_trans.getBasis()[2][1]; -// body1_mat[10] = body1_trans.getBasis()[2][2]; - - - // anchor points in global coordinates with respect to body PORs. - - int s = info->rowskip; // set jacobian @@ -160,17 +135,18 @@ void OdeP2PJoint::GetInfo2(Info2 *info) { for (int j=0; j<3; j++) { - info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); + info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } } else { for (int j=0; j<3; j++) { - info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - - body0_trans.getOrigin()[j]); - } + info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - body0_trans.getOrigin()[j]); + printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + + } } } diff --git a/src/BulletCollision/CollisionShapes/btBoxShape.h b/src/BulletCollision/CollisionShapes/btBoxShape.h index eaece37dc..30bc64556 100644 --- a/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -161,28 +161,22 @@ public: switch (i) { case 0: - plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 1: - plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); - plane[3] = -halfExtents.x(); + plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x()); break; case 2: - plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y()); break; case 3: - plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); - plane[3] = -halfExtents.y(); + plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y()); break; case 4: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z()); break; case 5: - plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); - plane[3] = -halfExtents.z(); + plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z()); break; default: assert(0); diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index c33d92459..e760347fb 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -23,7 +23,8 @@ Written by: Marcus Hennix #include btConeTwistConstraint::btConeTwistConstraint() -:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) +:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(true) { } @@ -31,7 +32,8 @@ btConeTwistConstraint::btConeTwistConstraint() btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(true) { m_swingSpan1 = btScalar(1e30); m_swingSpan2 = btScalar(1e30); @@ -46,7 +48,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false) + m_angularOnly(false), + m_useSolveConstraintObsolete(true) { m_rbBFrame = m_rbAFrame; @@ -61,221 +64,251 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& } + +void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + btAssert(0); + } +} + +void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(0); +} + + void btConeTwistConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - //set bias, sign, clear accumulator - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); - - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + m_appliedImpulse = btScalar(0.); - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) + //set bias, sign, clear accumulator + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + m_accTwistLimitImpulse = btScalar(0.); + m_accSwingLimitImpulse = btScalar(0.); + + if (!m_angularOnly) { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; - btPlaneSpace1(normal[0], normal[1], normal[2]); - - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; - - b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); - b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); - - btScalar swx=btScalar(0.),swy = btScalar(0.); - btScalar thresh = btScalar(10.); - btScalar fact; - - // Get Frame into world space - if (m_swingSpan1 >= btScalar(0.05f)) - { - b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); -// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; - - } - - if (m_swingSpan2 >= btScalar(0.05f)) - { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); -// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; - } - - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) - { - m_swingCorrection = EllipseAngle-1.0f; - m_solveSwingLimit = true; - - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - - m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); - - } - - // Twist limits - if (m_twistSpan >= btScalar(0.)) - { - btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); - - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - - m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + - getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); - - } else - if (twist > m_twistSpan*lockedFreeFactor) + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) { - m_twistCorrection = (twist - m_twistSpan); + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + } + } + + btVector3 b1Axis1,b1Axis2,b1Axis3; + btVector3 b2Axis1,b2Axis2; + + b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); + b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); + + btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + + btScalar swx=btScalar(0.),swy = btScalar(0.); + btScalar thresh = btScalar(10.); + btScalar fact; + + // Get Frame into world space + if (m_swingSpan1 >= btScalar(0.05f)) + { + b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); + // swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis2); + swing1 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing1 *= fact; + + } + + if (m_swingSpan2 >= btScalar(0.05f)) + { + b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); + // swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + swx = b2Axis1.dot(b1Axis1); + swy = b2Axis1.dot(b1Axis3); + swing2 = btAtan2Fast(swy, swx); + fact = (swy*swy + swx*swx) * thresh * thresh; + fact = fact / (fact + btScalar(1.0)); + swing2 *= fact; + } + + btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); + btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); + btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) + { + m_swingCorrection = EllipseAngle-1.0f; + m_solveSwingLimit = true; + + // Calculate necessary axis & factors + m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis.normalize(); + + btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + m_swingAxis *= swingAxisSign; + + m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + + } + + // Twist limits + if (m_twistSpan >= btScalar(0.)) + { + btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); + btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); + btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); + btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + + btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + if (twist <= -m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.normalize(); + m_twistAxis *= -1.0f; m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); - } + } else + if (twist > m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + + m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + + } + } } } -void btConeTwistConstraint::solveConstraint(btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { - - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - - btScalar tau = btScalar(0.3); - - //linear part - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; + btScalar tau = btScalar(0.3); - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - } - } - - { - ///solve angular part - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); - - // solve swing limit - if (m_solveSwingLimit) + //linear part + if (!m_angularOnly) { - btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); - btScalar impulseMag = amplitude * m_kSwing; + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; - btVector3 impulse = m_swingAxis * impulseMag; - - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + + } } - - // solve twist limit - if (m_solveTwistLimit) + { - btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); - btScalar impulseMag = amplitude * m_kTwist; + ///solve angular part + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; + // solve swing limit + if (m_solveSwingLimit) + { + btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); + btScalar impulseMag = amplitude * m_kSwing; - btVector3 impulse = m_twistAxis * impulseMag; + // Clamp the accumulated impulse + btScalar temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accSwingLimitImpulse - temp; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + btVector3 impulse = m_swingAxis * impulseMag; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_swingAxis,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_swingAxis,-impulseMag); + } + + // solve twist limit + if (m_solveTwistLimit) + { + btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); + btScalar impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + btScalar temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); + impulseMag = m_accTwistLimitImpulse - temp; + + btVector3 impulse = m_twistAxis * impulseMag; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); + + } + } - } } diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index ac3353a86..b91522480 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -63,6 +63,7 @@ public: bool m_solveTwistLimit; bool m_solveSwingLimit; + bool m_useSolveConstraintObsolete; public: @@ -74,7 +75,12 @@ public: virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 11715708d..30b310d07 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -22,7 +22,9 @@ enum btSolverMode SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, SOLVER_USE_FRICTION_WARMSTARTING = 8, - SOLVER_CACHE_FRIENDLY = 16 + SOLVER_CACHE_FRIENDLY = 16, + SOLVER_SIMD = 32,//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version + SOLVER_CUDA = 64 //will be open sourced during Game Developers Conference 2009. Much faster. }; struct btContactSolverInfoData @@ -72,7 +74,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_splitImpulsePenetrationThreshold = -0.02f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); - m_solverMode = SOLVER_CACHE_FRIENDLY | SOLVER_RANDMIZE_ORDER | SOLVER_USE_WARMSTARTING; + m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;//SOLVER_RANDMIZE_ORDER m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution } }; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index abcb8d753..1fc8dce3c 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -26,6 +26,35 @@ http://gimpact.sf.net #include +btGeneric6DofConstraint::btGeneric6DofConstraint() +:btTypedConstraint(D6_CONSTRAINT_TYPE), +m_useLinearReferenceFrameA(true), +m_useSolveConstraintObsolete(true) +{ +} + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) +: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB), +m_useLinearReferenceFrameA(useLinearReferenceFrameA), +m_useSolveConstraintObsolete(true) +{ + +} + + + +#define dCROSSMAT(A,a,skip,plus,minus) \ +{ \ + (A)[1] = minus (a)[2]; \ + (A)[2] = plus (a)[1]; \ + (A)[(skip)+0] = plus (a)[2]; \ + (A)[(skip)+2] = minus (a)[0]; \ + (A)[2*(skip)+0] = minus (a)[1]; \ + (A)[2*(skip)+1] = plus (a)[0]; \ +} + #define GENERIC_D6_DISABLE_WARMSTARTING 1 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); @@ -40,37 +69,37 @@ btScalar btGetMatrixElem(const btMatrix3x3& mat, int index) bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); bool 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 -// + // // 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 + // - if (btGetMatrixElem(mat,2) < btScalar(1.0)) + if (btGetMatrixElem(mat,2) < btScalar(1.0)) + { + if (btGetMatrixElem(mat,2) > btScalar(-1.0)) { - if (btGetMatrixElem(mat,2) > btScalar(-1.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[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } + 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. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btScalar(0.0); + return false; } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = SIMD_HALF_PI; + xyz[2] = 0.0; + + } return false; @@ -104,85 +133,89 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) m_currentLimit = 0;//Free from violation return 0; - + } btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1) + btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, + btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) { - if (needApplyTorques()==false) return 0.0f; + if (needApplyTorques()==false) return 0.0f; - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; + btScalar target_velocity = m_targetVelocity; + btScalar maxMotorForce = m_maxMotorForce; //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_ERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } + if (m_currentLimit!=0) + { + target_velocity = -m_ERP*m_currentLimitError/(timeStep); + maxMotorForce = m_maxLimitForce; + } - maxMotorForce *= timeStep; + maxMotorForce *= timeStep; - // current velocity difference - btVector3 vel_diff = body0->getAngularVelocity(); - if (body1) - { - vel_diff -= body1->getAngularVelocity(); - } + // current velocity difference + + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); + + btVector3 vel_diff; + vel_diff = angVelA-angVelB; - btScalar rel_vel = axis.dot(vel_diff); + btScalar rel_vel = axis.dot(vel_diff); // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); + btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) - { - return 0.0f;//no need for applying force - } + if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) + { + return 0.0f;//no need for applying force + } // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; + btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; // clip correction impulse - btScalar clippedMotorImpulse; + btScalar clippedMotorImpulse; - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } + ///@todo: should clip against accumulated impulse + if (unclippedMotorImpulse>0.0f) + { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; + } + else + { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; + } // sort with accumulated impulses - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + btScalar oldaccumImpulse = m_accumulatedImpulse; + btScalar sum = oldaccumImpulse + clippedMotorImpulse; + m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; + + btVector3 motorImp = clippedMotorImpulse * axis; + + //body0->applyTorqueImpulse(motorImp); + //body1->applyTorqueImpulse(-motorImp); + + bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - - btVector3 motorImp = clippedMotorImpulse * axis; - - - body0->applyTorqueImpulse(motorImp); - if (body1) body1->applyTorqueImpulse(-motorImp); - - return clippedMotorImpulse; + return clippedMotorImpulse; } @@ -191,98 +224,94 @@ btScalar btRotationalLimitMotor::solveAngularLimits( //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) + btScalar timeStep, + btScalar jacDiagABInv, + btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, + btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + int limit_index, + const btVector3 & axis_normal_on_a, + const btVector3 & anchorPos) { -///find relative velocity -// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); -// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); - btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); + ///find relative velocity + // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); + // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); + btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; + btVector3 vel1; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + btVector3 vel2; + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; - btScalar rel_vel = axis_normal_on_a.dot(vel); + btScalar rel_vel = axis_normal_on_a.dot(vel); -/// apply displacement correction + /// apply displacement correction -//positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-1e30); - btScalar hi = btScalar(1e30); + //positional error (zeroth order error) + btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); + btScalar lo = btScalar(-1e30); + btScalar hi = btScalar(1e30); - btScalar minLimit = m_lowerLimit[limit_index]; - btScalar maxLimit = m_upperLimit[limit_index]; + btScalar minLimit = m_lowerLimit[limit_index]; + btScalar maxLimit = m_upperLimit[limit_index]; - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = btScalar(0.); + //handle the limits + if (minLimit < maxLimit) + { + { + if (depth > maxLimit) + { + depth -= maxLimit; + lo = btScalar(0.); - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = btScalar(0.); - } - else - { - return 0.0f; - } - } - } - } + } + else + { + if (depth < minLimit) + { + depth -= minLimit; + hi = btScalar(0.); + } + else + { + return 0.0f; + } + } + } + } - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; + btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; + btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; + btScalar sum = oldNormalImpulse + normalImpulse; + m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; + normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1.applyImpulse( impulse_vector, rel_pos1); - body2.applyImpulse(-impulse_vector, rel_pos2); - return normalImpulse; + btVector3 impulse_vector = axis_normal_on_a * normalImpulse; + //body1.applyImpulse( impulse_vector, rel_pos1); + //body2.applyImpulse(-impulse_vector, rel_pos2); + + btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); + btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); + bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + + + + + return normalImpulse; } //////////////////////////// btTranslationalLimitMotor //////////////////////////////////// -btGeneric6DofConstraint::btGeneric6DofConstraint() - :btTypedConstraint(D6_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true) -{ -} - -btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) - , m_frameInA(frameInA) - , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA) -{ - -} - @@ -296,19 +325,19 @@ void btGeneric6DofConstraint::calculateAngleInfo() // 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]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); @@ -318,189 +347,459 @@ void btGeneric6DofConstraint::calculateAngleInfo() m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); -// if(m_debugDrawer) -// { -// -// char buff[300]; -// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", -// m_calculatedAxisAngleDiff[0], -// m_calculatedAxisAngleDiff[1], -// m_calculatedAxisAngleDiff[2]); -// m_debugDrawer->reportErrorWarning(buff); -// } + // if(m_debugDrawer) + // { + // + // char buff[300]; + // sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", + // m_calculatedAxisAngleDiff[0], + // m_calculatedAxisAngleDiff[1], + // m_calculatedAxisAngleDiff[2]); + // m_debugDrawer->reportErrorWarning(buff); + // } } void btGeneric6DofConstraint::calculateTransforms() { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - calculateAngleInfo(); + calculateAngleInfo(); } void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) + btJacobianEntry & jacLinear,const btVector3 & normalWorld, + const btVector3 & pivotAInW,const btVector3 & pivotBInW) { - new (&jacLinear) btJacobianEntry( + new (&jacLinear) btJacobianEntry( - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normalWorld, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); } void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) + btJacobianEntry & jacAngular,const btVector3 & jointAxisW) { - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + new (&jacAngular) btJacobianEntry(jointAxisW, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); } bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); + //test limits + m_angularLimits[axis_index].testLimitValue(angle); + return m_angularLimits[axis_index].needApplyTorques(); } void btGeneric6DofConstraint::buildJacobian() { + if (m_useSolveConstraintObsolete) + { - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); - } - //calculates transform - calculateTransforms(); + // Clear accumulated impulses for the next simulation step + m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + int i; + for(i = 0; i < 3; i++) + { + m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); + } + //calculates transform + calculateTransforms(); -// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); -// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); - calcAnchorPos(); - btVector3 pivotAInW = m_AnchorPos; - btVector3 pivotBInW = m_AnchorPos; + // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); + // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); + calcAnchorPos(); + btVector3 pivotAInW = m_AnchorPos; + btVector3 pivotBInW = m_AnchorPos; -// not used here -// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); -// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + // not used here + // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - btVector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - else - normalWorld = m_calculatedTransformB.getBasis().getColumn(i); + btVector3 normalWorld; + //linear part + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + if (m_useLinearReferenceFrameA) + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + else + normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); + buildLinearJacobian( + m_jacLinear[i],normalWorld , + pivotAInW,pivotBInW); - } - } + } + } - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); - } - } + // angular part + for (i=0;i<3;i++) + { + //calculates error angle + if (testAngularLimitMotor(i)) + { + normalWorld = this->getAxis(i); + // Create angular atom + buildAngularJacobian(m_jacAng[i],normalWorld); + } + } + } +} + +void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + //prepare constraint + calculateTransforms(); + info->m_numConstraintRows = 3; + info->nub = 3;//?? + //test angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(testAngularLimitMotor(i)) + { + info->m_numConstraintRows++; + } + } + } +} + +void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + + int row = setLinearLimits(info); + setAngularLimits(info, row); +} + +int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info) +{ + + btGeneric6DofConstraint * d6constraint = this; + + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + + btTransform body1_trans; + + body1_trans = m_rbB.getCenterOfMassTransform(); + + // anchor points in global coordinates with respect to body PORs. + + int s = info->rowskip; + + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[s+1] = 1; + info->m_J1linearAxis[2*s+2] = 1; + + + btVector3 a1,a2; + + a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->m_J1angularAxis,a1,s,-,+); + + /*info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[s+1] = -1; + info->m_J2linearAxis[2*s+2] = -1; + */ + + a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); + + // set right hand side + btScalar k = info->fps * info->erp; + for (int j=0; j<3; j++) + { + info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] - + a1[j] - body0_trans.getOrigin()[j]); + } + + return 3; } -void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +/*! \pre testLimitValue must be called on limot*/ +int bt_get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btTypedConstraint::btConstraintInfo2 *info, int row, btVector3& ax1, int rotational) { - m_timeStep = timeStep; - //calculateTransforms(); - int i; + int srow = row * info->rowskip; - // linear + // if the joint is powered, or has joint limits, add in the extra row + int powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; - btVector3 pointInA = m_calculatedTransformA.getOrigin(); - btVector3 pointInB = m_calculatedTransformB.getOrigin(); - - btScalar jacDiagABInv; - btVector3 linear_axis; - for (i=0;i<3;i++) + if (powered || limit) { - if (m_linearLimits.isLimited(i)) + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : 0;//info->m_J2linearAxis; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (body1) { - jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); - - if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.getBasis().getColumn(i); - else - linear_axis = m_calculatedTransformB.getBasis().getColumn(i); - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - m_rbA,pointInA, - m_rbB,pointInB, - i,linear_axis, m_AnchorPos); - + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; } - } - // angular - btVector3 angular_axis; - btScalar angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].needApplyTorques()) + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), we have to be careful + // that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + btVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && body1) { + btVector3 c; + c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0] + -body0->getCenterOfMassPosition()[0]); + c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1] + -body0->getCenterOfMassPosition()[1]); + c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2] + -body0->getCenterOfMassPosition()[2]); - // get axis - angular_axis = getAxis(i); + ltd = c.cross(ax1); - angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); - - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB); + info->m_J1angularAxis[srow+0] = ltd[0]; + info->m_J1angularAxis[srow+1] = ltd[1]; + info->m_J1angularAxis[srow+2] = ltd[2]; + info->m_J2angularAxis[srow+0] = ltd[0]; + info->m_J2angularAxis[srow+1] = ltd[1]; + info->m_J2angularAxis[srow+2] = ltd[2]; } + + // if we're limited low and high simultaneously, the joint motor is + // ineffective + + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; + + if (powered) + { + info->cfm[srow] = 0.0f;//limot->m_normalCFM; + if (! limit) + { + info->m_constraintError[srow] = limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce; + info->m_upperLimit[srow] = limot->m_maxMotorForce; + } + } + + if (limit) + { + btScalar k = info->fps * limot->m_ERP; + info->m_constraintError[srow] = -k * limot->m_currentLimitError; + info->cfm[srow] = 0.0f;//limot->m_stopCFM; + + if (limot->m_loLimit == limot->m_hiLimit) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + if (limit == 1) + { + // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { + // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + btScalar vel; + if (rotational) + { + vel = body0->getAngularVelocity().dot(ax1); + if (body1) + vel -= body1->getAngularVelocity().dot(ax1); + } + else + { + vel = body0->getLinearVelocity().dot(ax1); + if (body1) + vel -= body1->getLinearVelocity().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) + { + btScalar newc = -limot->m_bounce* vel; + if (newc > info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + else + { + // high limit - all those computations are reversed + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + } + } + } + return 1; } + else return 0; +} + + + +int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset) +{ + btGeneric6DofConstraint * d6constraint = this; + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + { + btVector3 axis = d6constraint->getAxis(i); + row += bt_get_limit_motor_info2( + d6constraint->getRotationalLimitMotor(i), + &m_rbA, + &m_rbB, + info,row,axis,1); + } + } + + return row; +} + +///////////////////limit motor support + + +void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + + + m_timeStep = timeStep; + + //calculateTransforms(); + + int i; + + // linear + + btVector3 pointInA = m_calculatedTransformA.getOrigin(); + btVector3 pointInB = m_calculatedTransformB.getOrigin(); + + btScalar jacDiagABInv; + btVector3 linear_axis; + for (i=0;i<3;i++) + { + if (m_linearLimits.isLimited(i)) + { + jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); + + if (m_useLinearReferenceFrameA) + linear_axis = m_calculatedTransformA.getBasis().getColumn(i); + else + linear_axis = m_calculatedTransformB.getBasis().getColumn(i); + + m_linearLimits.solveLinearAxis( + m_timeStep, + jacDiagABInv, + m_rbA,bodyA,pointInA, + m_rbB,bodyB,pointInB, + i,linear_axis, m_AnchorPos); + + } + } + + // angular + btVector3 angular_axis; + btScalar angularJacDiagABInv; + for (i=0;i<3;i++) + { + if (m_angularLimits[i].needApplyTorques()) + { + + // get axis + angular_axis = getAxis(i); + + angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); + + m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); + } + } + } } void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { - (void)timeStep; + (void)timeStep; } btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const { - return m_calculatedAxis[axis_index]; + return m_calculatedAxis[axis_index]; } btScalar btGeneric6DofConstraint::getAngle(int axis_index) const { - return m_calculatedAxisAngleDiff[axis_index]; + return m_calculatedAxisAngleDiff[axis_index]; } void btGeneric6DofConstraint::calcAnchorPos(void) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index f0718d2d4..875d108b2 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -110,7 +110,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); }; @@ -168,8 +168,8 @@ public: btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, + btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, + btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos); @@ -262,6 +262,9 @@ protected: } + int setAngularLimits(btConstraintInfo2 *info, int row_offset); + + int setLinearLimits(btConstraintInfo2 *info); void buildLinearJacobian( btJacobianEntry & jacLinear,const btVector3 & normalWorld, @@ -276,6 +279,10 @@ protected: public: + + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btGeneric6DofConstraint(); @@ -330,7 +337,11 @@ public: //! performs Jacobian calculation, and also calculates angle differences and axis virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index ae8ba96a7..5f576f12a 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,11 +19,14 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" #include "LinearMath/btMinMax.h" #include +#include "btSolverBody.h" + btHingeConstraint::btHingeConstraint() : btTypedConstraint (HINGE_CONSTRAINT_TYPE), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(true) { } @@ -31,7 +34,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btVector3& axisInA,btVector3& axisInB) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), - m_enableAngularMotor(false) + m_enableAngularMotor(false), + m_useSolveConstraintObsolete(true) { m_rbAFrame.getOrigin() = pivotInA; @@ -77,7 +81,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(true) { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -115,7 +119,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(true) { // flip axis m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); @@ -136,7 +141,8 @@ m_enableAngularMotor(false) btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), -m_enableAngularMotor(false) +m_enableAngularMotor(false), +m_useSolveConstraintObsolete(true) { ///not providing rigidbody B means implicitly using worldspace for body B @@ -158,226 +164,270 @@ m_enableAngularMotor(false) void btHingeConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - if (!m_angularOnly) + if (m_useSolveConstraintObsolete) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; + m_appliedImpulse = btScalar(0.); - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) + if (!m_angularOnly) { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + } } - btPlaneSpace1(normal[0], normal[1], normal[2]); + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + //this is unused for now, it's a todo + btVector3 jointAxis0local; + btVector3 jointAxis1local; - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + 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[2]) btJacobianEntry(hingeAxisWorld, - 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()); - // Compute limit information - btScalar hingeAngle = getHingeAngle(); + // Compute limit information + btScalar hingeAngle = getHingeAngle(); - //set bias, sign, clear accumulator - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - m_accLimitImpulse = btScalar(0.); + //set bias, sign, clear accumulator + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + m_accLimitImpulse = btScalar(0.); -// if (m_lowerLimit < m_upperLimit) - if (m_lowerLimit <= m_upperLimit) - { -// if (hingeAngle <= m_lowerLimit*m_limitSoftness) - if (hingeAngle <= m_lowerLimit) + // if (m_lowerLimit < m_upperLimit) + if (m_lowerLimit <= m_upperLimit) { - m_correction = (m_lowerLimit - hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } -// else if (hingeAngle >= m_upperLimit*m_limitSoftness) - else if (hingeAngle >= m_upperLimit) - { - m_correction = m_upperLimit - hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; + // if (hingeAngle <= m_lowerLimit*m_limitSoftness) + if (hingeAngle <= m_lowerLimit) + { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + // else if (hingeAngle >= m_upperLimit*m_limitSoftness) + else if (hingeAngle >= m_upperLimit) + { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } } + + //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)); + } - - //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)); - } -void btHingeConstraint::solveConstraint(btScalar timeStep) + +void btHingeConstraint::getInfo1 (btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + btAssert(0); + } +} + +void btHingeConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(0); +} + + +void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - - btScalar tau = btScalar(0.3); - - //linear part - if (!m_angularOnly) + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + if (m_useSolveConstraintObsolete) { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + btScalar tau = btScalar(0.3); - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - } - } - - - { - ///solve angular part - - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - - const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); - const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); - - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; + //linear part + if (!m_angularOnly) { - //solve orthogonal angular velocity correction - btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - } + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * relaxation; - } + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; - m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); - m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; - - btScalar impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; - - - btVector3 impulse = axisA * impulseMag * m_limitSign; - m_rbA.applyTorqueImpulse(impulse); - m_rbB.applyTorqueImpulse(-impulse); + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } - //apply motor - if (m_enableAngularMotor) + { - //todo: add limits too - btVector3 angularLimit(0,0,0); + ///solve angular part - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - //todo: should clip against accumulated impulse - btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; - clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; - btVector3 motorImp = clippedMotorImpulse * axisA; + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - m_rbA.applyTorqueImpulse(motorImp+angularLimit); - m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; + { + + + //solve orthogonal angular velocity correction + btScalar relaxation = btScalar(1.); + btScalar len = velrelOrthog.length(); + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); + + } + + //solve angular positional correction + btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + //angularError *= (btScalar(1.)/denom2) * relaxation; + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); + + } + + + + + + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); + impulseMag = m_accLimitImpulse - temp; + + + + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); + + } + } + + //apply motor + if (m_enableAngularMotor) + { + //todo: add limits too + btVector3 angularLimit(0,0,0); + + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; + + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); + bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); + + } } } diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 4fa9972f6..6405381c6 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -57,6 +57,7 @@ public: bool m_angularOnly; bool m_enableAngularMotor; bool m_solveLimit; + bool m_useSolveConstraintObsolete; public: @@ -73,7 +74,11 @@ public: virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 5046328a2..03ca2334b 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -21,100 +21,197 @@ subject to the following restrictions: btPoint2PointConstraint::btPoint2PointConstraint() -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_useSolveConstraintObsolete(false) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_useSolveConstraintObsolete(false) { } void btPoint2PointConstraint::buildJacobian() { - m_appliedImpulse = btScalar(0.); - - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) + if (m_useSolveConstraintObsolete) { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( + m_appliedImpulse = btScalar(0.); - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; + btVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( + + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } } } -void btPoint2PointConstraint::solveConstraint(btScalar timeStep) + +void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } else + { + info->m_numConstraintRows = 3; + info->nub = 3; + } +} +#define dCROSSMAT(A,a,skip,plus,minus) \ +{ \ + (A)[1] = minus (a)[2]; \ + (A)[2] = plus (a)[1]; \ + (A)[(skip)+0] = plus (a)[2]; \ + (A)[(skip)+2] = minus (a)[0]; \ + (A)[2*(skip)+0] = minus (a)[1]; \ + (A)[2*(skip)+1] = plus (a)[0]; \ +} +#include +void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); - btVector3 normal(0,0,0); - + //retrieve matrices + btTransform body0_trans; + body0_trans = m_rbA.getCenterOfMassTransform(); + btTransform body1_trans; + body1_trans = m_rbB.getCenterOfMassTransform(); -// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); -// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + // anchor points in global coordinates with respect to body PORs. + int s = info->rowskip; - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[s+1] = 1; + info->m_J1linearAxis[2*s+2] = 1; - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel; - rel_vel = normal.dot(vel); + btVector3 a1,a2; - /* - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); + a1 = body0_trans.getBasis()*getPivotInA(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->m_J1angularAxis,a1,s,-,+); + + /*info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[s+1] = -1; + info->m_J2linearAxis[2*s+2] = -1; */ - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; - btScalar impulseClamp = m_setting.m_impulseClamp; + a2 = body1_trans.getBasis()*getPivotInB(); + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + //dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); + dCROSSMAT (info->m_J2angularAxis,a2,s,+,-); + + + + // set right hand side + btScalar k = info->fps * info->erp; + int j; + + for (j=0; j<3; j++) + { + info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + btScalar impulseClamp = m_setting.m_impulseClamp; + for (j=0; j<3; j++) + { if (impulseClamp > 0) { - if (impulse < -impulseClamp) - impulse = -impulseClamp; - if (impulse > impulseClamp) - impulse = impulseClamp; + info->m_lowerLimit[j*s] = -impulseClamp; + info->m_upperLimit[j*s] = impulseClamp; } + } + +} - m_appliedImpulse+=impulse; - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + +void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + btVector3 normal(0,0,0); - normal[i] = 0; + + // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1,vel2; + bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + + btScalar impulseClamp = m_setting.m_impulseClamp; + if (impulseClamp > 0) + { + if (impulse < -impulseClamp) + impulse = -impulseClamp; + if (impulse > impulseClamp) + impulse = impulseClamp; + } + + m_appliedImpulse+=impulse; + btVector3 impulse_vector = normal * impulse; + + btVector3 ftorqueAxis1 = rel_pos1.cross(normal); + btVector3 ftorqueAxis2 = rel_pos2.cross(normal); + bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + + + normal[i] = 0; + } } } diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index c9d596853..e2b865cd4 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -50,6 +50,9 @@ public: public: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btConstraintSetting m_setting; btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); @@ -60,8 +63,12 @@ public: virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo2 (btConstraintInfo2* info); + + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 381592d79..78209a11b 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -37,29 +37,73 @@ subject to the following restrictions: btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() :m_btSeed2(0) { + } btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() { } -// Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +#ifdef USE_SIMD +#include +#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) +static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) { - float deltaImpulse; - deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm; - btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); - btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); - btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + __m128 result = _mm_mul_ps( vec0, vec1); + return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); +} +#endif//USE_SIMD + +// Project Gauss Seidel or the equivalent Sequential Impulse +SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128)); + __m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_add_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass)); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse +SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv; - - btScalar sum = c.m_appliedImpulse + deltaImpulse; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) { deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; - } + } else if (sum > c.m_upperLimit) { deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; @@ -69,8 +113,71 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBod { c.m_appliedImpulse = sum; } - body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse); + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse); +} + + +SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ +#ifdef USE_SIMD + + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128)); + __m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_add_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + + btSimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass)); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass)); + //body1.applyImpulse(c.m_contactNormal*body1.m_invMass.m_vec128,c.m_angularComponentA,deltaImpulse); + //body2.applyImpulse(c.m_contactNormal*body2.m_invMass.m_vec128,c.m_angularComponentB,-deltaImpulse); + __m128 impulseMagnitude = deltaImpulse; + body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + +#else + resolveSingleConstraintRowLowerLimit(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse +SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +{ + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); + const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + if (body1.m_invMass) + body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); + if (body2.m_invMass) + body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse); } @@ -113,39 +220,24 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) - - - - - void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { - btRigidBody* rb = btRigidBody::upcast(collisionObject); + btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); if (rb) { - solverBody->m_angularVelocity = rb->getAngularVelocity() ; - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = rb->getInvMass(); - solverBody->m_linearVelocity = rb->getLinearVelocity(); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); } else { - solverBody->m_angularVelocity.setValue(0,0,0); - solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin(); - solverBody->m_friction = collisionObject->getFriction(); solverBody->m_invMass = 0.f; - solverBody->m_linearVelocity.setValue(0,0,0); solverBody->m_originalBody = 0; solverBody->m_angularFactor = 1.f; } - solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); - solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); } @@ -157,149 +249,19 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, return rest; } -//SIMD_FORCE_INLINE -void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo) -{ - (void)solverInfo; - if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold) - { - - gNumSplitImpulseRecoveries++; - btScalar normalImpulse; - - // Optimized version of projected relative velocity, use precomputed cross products with normal - // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); - // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); - // btVector3 vel = vel1 - vel2; - // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); - - btScalar rel_vel; - btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity); - btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity); - - rel_vel = vel1Dotn-vel2Dotn; - - - btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep; - // btScalar positionalError = contactConstraint.m_penetration; - - btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; - normalImpulse = penetrationImpulse+velocityImpulse; - - // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse - btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse; - btScalar sum = oldNormalImpulse + normalImpulse; - contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum; - - normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse; - - body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse); - - body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse); - - } - -} - - - - - -//SIMD_FORCE_INLINE -void btSequentialImpulseConstraintSolver::resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse) -{ - (void)solverInfo; - - const btScalar combinedFriction = contactConstraint.m_friction; - - const btScalar limit = appliedNormalImpulse * combinedFriction; - - if (appliedNormalImpulse>btScalar(0.)) - //friction - { - - btScalar j1; - { - -#ifndef _USE_JACOBIAN - btScalar rel_vel; - const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) - + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); - const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) - + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); - - rel_vel = vel1Dotn-vel2Dotn; -#else - btScalar rel_vel = contactConstraint.m_jac.getRelativeVelocity(body1.m_linearVelocity1+body1.m_deltaLinearVelocity,body1.m_angularVelocity1+body1.m_deltaAngularVelocity, - body2.m_linearVelocity1+body2.m_deltaLinearVelocity,body2.m_angularVelocity1+body2.m_deltaAngularVelocity); -#endif //_USE_JACOBIAN - - // calculate j that moves us to zero relative velocity - j1 = -rel_vel * contactConstraint.m_jacDiagABInv; -#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1 -#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE - btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; - contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; - - if (limit < contactConstraint.m_appliedImpulse) - { - contactConstraint.m_appliedImpulse = limit; - } else - { - if (contactConstraint.m_appliedImpulse < -limit) - contactConstraint.m_appliedImpulse = -limit; - } - j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; -#else - if (limit < j1) - { - j1 = limit; - } else - { - if (j1 < -limit) - j1 = -limit; - } - -#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE - - //GEN_set_min(contactConstraint.m_appliedImpulse, limit); - //GEN_set_max(contactConstraint.m_appliedImpulse, -limit); - - - - } - - body1.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); - - body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); - - } -} btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) { + btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); - btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); + memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_solverBodyIdA = solverBodyIdA; @@ -310,8 +272,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedPushImpulse = 0.f; + solverConstraint.m_appliedImpulse = 0.f; +// solverConstraint.m_appliedPushImpulse = 0.f; solverConstraint.m_penetration = 0.f; { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); @@ -355,10 +317,57 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c body1->getInvInertiaDiagLocal(), body1->getInvMass()); #endif //_USE_JACOBIAN + + + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); + + rel_vel = vel1Dotn-vel2Dotn; + + btScalar positionalError = 0.f; + positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + solverConstraint.m_restitution=0.f; + + btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + return solverConstraint; } +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) +{ + int solverBodyIdA = -1; + if (body.getCompanionId() >= 0) + { + //body has already been converted + solverBodyIdA = body.getCompanionId(); + } else + { + btRigidBody* rb = btRigidBody::upcast(&body); + if (rb && rb->getInvMass()) + { + solverBodyIdA = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,&body); + body.setCompanionId(solverBodyIdA); + } else + { + return 0;//assume first one is a fixed solver body + } + } + return solverBodyIdA; +} +#include btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { @@ -372,16 +381,149 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol // printf("empty\n"); return 0.f; } - btPersistentManifold* manifold = 0; - btCollisionObject* colObj0=0,*colObj1=0; + + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0); //btRigidBody* rb0=0,*rb1=0; //if (1) { + { + + int totalNumRows = 0; + + //calculate the total number of contraint rows + for (int i=0;igetInfo1(&info1); + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resize(totalNumRows); + + btTypedConstraint::btConstraintInfo1 info1; + info1.m_numConstraintRows = 0; + + if (m_tmpSolverNonContactConstraintPool.size()>3) + { + printf("dsadas\n"); + } + ///setup the btSolverConstraints + int currentRow = 0; + + for (int i=0;igetInfo1(&info1); + if (info1.m_numConstraintRows) + { + btAssert(currentRowgetRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = getOrInitSolverBody(rbA); + int solverBodyIdB = getOrInitSolverBody(rbB); + + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + for (int j=0;jm_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + + + + btTypedConstraint::btConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + info2.m_constraintError = ¤tConstraintRow->m_rhs; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + constraints[i]->getInfo2(&info2); + + ///finalize the constraint setup + for (int j=0;jgetRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2; + } + + { + btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + + solverConstraint.m_jacDiagABInv = 1./sum; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); + btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + + rel_vel = vel1Dotn-vel2Dotn; + + btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + solverConstraint.m_restitution = 0.f; + btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + } + } + } { int i; + btPersistentManifold* manifold = 0; + btCollisionObject* colObj0=0,*colObj1=0; + for (i=0;igetNumContacts()) { - - - - if (colObj0->getIslandTag() >= 0) - { - if (colObj0->getCompanionId() >= 0) - { - //body has already been converted - solverBodyIdA = colObj0->getCompanionId(); - } else - { - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); - colObj0->setCompanionId(solverBodyIdA); - } - } else - { - //create a static body - solverBodyIdA = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj0); - } - - if (colObj1->getIslandTag() >= 0) - { - if (colObj1->getCompanionId() >= 0) - { - solverBodyIdB = colObj1->getCompanionId(); - } else - { - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); - colObj1->setCompanionId(solverBodyIdB); - } - } else - { - //create a static body - solverBodyIdB = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,colObj1); - } + solverBodyIdA = getOrInitSolverBody(*colObj0); + solverBodyIdB = getOrInitSolverBody(*colObj1); } btVector3 rel_pos1; @@ -463,10 +564,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar rel_vel; btVector3 vel; - int frictionIndex = m_tmpSolverConstraintPool.size(); + int frictionIndex = m_tmpSolverContactConstraintPool.size(); { - btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb1 = btRigidBody::upcast(colObj1); @@ -541,7 +642,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol }; } - + ///warm starting (or zero if disabled) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { @@ -555,7 +656,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol solverConstraint.m_appliedImpulse = 0.f; } - solverConstraint.m_appliedPushImpulse = 0.f; +// solverConstraint.m_appliedPushImpulse = 0.f; { btScalar rel_vel; @@ -568,14 +669,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar positionalError = 0.f; positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; - btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; - - btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = (penetrationImpulse+velocityImpulse); + btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e30f; + solverConstraint.m_upperLimit = 1e10f; } @@ -594,7 +694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (1) { - solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); if (!cp.m_lateralFrictionInitialized) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; @@ -615,9 +715,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol //re-calculate friction direction every frame, todo: check if this is really needed btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) { + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } } @@ -632,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) { { - btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex]; + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; @@ -646,7 +746,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } { - btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; @@ -672,6 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btContactSolverInfo info = infoGlobal; + if (1) { int j; for (j=0;jgetRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + ///solve all joint constraints, using SIMD, if available + for (j=0;jgetRigidBodyA().getCompanionId()].writebackVelocity(); - } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) - { - m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity(); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); } - constraint->solveConstraint(infoGlobal.m_timeStep); - - if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0)) + for (j=0;jgetRigidBodyA().getCompanionId()].readVelocity(); - } - if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) - { - m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } - } - - - { - int numPoolConstraints = m_tmpSolverConstraintPool.size(); + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); for (j=0;jbtScalar(0)) { - const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], - m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } else + { + + ///solve all joint constraints + for (j=0;jgetRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } + + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], + m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } } + + } - } - return 0.f; } @@ -830,23 +946,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - int numPoolConstraints = m_tmpSolverConstraintPool.size(); + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) { - pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; } //do a callback here? - } if (infoGlobal.m_splitImpulse) @@ -865,8 +981,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod m_tmpSolverBodyPool.resize(0); - m_tmpSolverConstraintPool.resize(0); - m_tmpSolverFrictionConstraintPool.resize(0); + m_tmpSolverContactConstraintPool.resize(0); + m_tmpSolverNonContactConstraintPool.resize(0); + m_tmpSolverContactFrictionConstraintPool.resize(0); return 0.f; } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index fb16fdef2..f1c1c6ddd 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -23,6 +23,7 @@ class btIDebugDraw; #include "btSolverConstraint.h" + ///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses ///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com ///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) @@ -31,12 +32,12 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver { btAlignedObjectArray m_tmpSolverBodyPool; - btAlignedObjectArray m_tmpSolverConstraintPool; - btAlignedObjectArray m_tmpSolverFrictionConstraintPool; + btConstraintArray m_tmpSolverContactConstraintPool; + btConstraintArray m_tmpSolverNonContactConstraintPool; + btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray m_orderTmpConstraintPool; btAlignedObjectArray m_orderFrictionConstraintPool; - protected: btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation); @@ -52,18 +53,17 @@ protected: const btSolverConstraint& contactConstraint, const btContactSolverInfo& solverInfo); - void resolveSingleConstraintRow( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint); + //internal method + int getOrInitSolverBody(btCollisionObject& body); - void resolveSingleFrictionCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& contactConstraint, - const btContactSolverInfo& solverInfo, - btScalar appliedNormalImpulse); + void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + public: diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 2abe61e2a..990bd00e9 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -153,27 +153,41 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraint(btScalar timeStep) +void btSliderConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 0; + info->nub = 0; +} + +void btSliderConstraint::getInfo2 (btConstraintInfo2* info) +{ + btAssert(0); +} + + +void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { m_timeStep = timeStep; if(m_useLinearReferenceFrameA) { - solveConstraintInt(m_rbA, m_rbB); + solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); } else { - solveConstraintInt(m_rbB, m_rbA); + solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); } } // btSliderConstraint::solveConstraint() //----------------------------------------------------------------------------- -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) { int i; // linear - btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); - btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); + btVector3 velA; + bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); + btVector3 velB; + bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); btVector3 vel = velA - velB; for(i = 0; i < 3; i++) { @@ -188,8 +202,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) // calcutate and apply impulse btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btVector3 impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + if(m_poweredLinMotor && (!i)) { // apply linear motor if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) @@ -215,8 +239,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedLinMotorImpulse = new_acc; // apply clamped impulse impulse_vector = normal * normalImpulse; - rbA.applyImpulse( impulse_vector, m_relPosA); - rbB.applyImpulse(-impulse_vector, m_relPosB); + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + } } } @@ -225,8 +259,10 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - const btVector3& angVelA = rbA.getAngularVelocity(); - const btVector3& angVelB = rbB.getAngularVelocity(); + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); @@ -236,24 +272,38 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) btVector3 velrelOrthog = angAorthog-angBorthog; //solve orthogonal angular velocity correction btScalar len = velrelOrthog.length(); + btScalar orthorImpulseMag = 0.f; + if (len > btScalar(0.00001)) { btVector3 normal = velrelOrthog.normalized(); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; } //solve angular positional correction btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btVector3 angularAxis = angularError; + btScalar angularImpulseMag = 0; + btScalar len2 = angularError.length(); if (len2>btScalar(0.00001)) { btVector3 normal2 = angularError.normalized(); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularError *= angularImpulseMag; } // apply impulse - rbA.applyTorqueImpulse(-velrelOrthog+angularError); - rbB.applyTorqueImpulse(velrelOrthog-angularError); + //rbA.applyTorqueImpulse(-velrelOrthog+angularError); + //rbB.applyTorqueImpulse(velrelOrthog-angularError); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); + + btScalar impulseMag; //solve angular limits if(m_solveAngLim) @@ -267,8 +317,14 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) impulseMag *= m_kAngle * m_softnessDirAng; } btVector3 impulse = axisA * impulseMag; - rbA.applyTorqueImpulse(impulse); - rbB.applyTorqueImpulse(-impulse); + //rbA.applyTorqueImpulse(impulse); + //rbB.applyTorqueImpulse(-impulse); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); + + + //apply angular motor if(m_poweredAngMotor) { @@ -299,8 +355,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) m_accumulatedAngMotorImpulse = new_acc; // apply clamped impulse btVector3 motorImp = angImpulse * axisA; - m_rbA.applyTorqueImpulse(motorImp); - m_rbB.applyTorqueImpulse(-motorImp); + //rbA.applyTorqueImpulse(motorImp); + //rbB.applyTorqueImpulse(-motorImp); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); } } } // btSliderConstraint::solveConstraint() diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 7f8d40948..94baf61ad 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -126,7 +126,13 @@ public: btSliderConstraint(); // overrides virtual void buildJacobian(); - virtual void solveConstraint(btScalar timeStep); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + + // access const btRigidBody& getRigidBodyA() const { return m_rbA; } const btRigidBody& getRigidBodyB() const { return m_rbB; } @@ -202,7 +208,7 @@ public: btScalar getAngDepth() { return m_angDepth; } // internal void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); + void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); // shared code used by ODE solver void calculateTransforms(void); void testLinLimits(void); diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 5b812a0d0..cab8a56a5 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -23,193 +23,157 @@ class btRigidBody; #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" -#ifndef _USE_JACOBIAN +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) +#define USE_SIMD 1 +#endif // -///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody + +#ifdef USE_SIMD +#include + +struct btSimdScalar { - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMatrix3x3 m_worldInvInertiaTensor; - - btVector3 m_angularVelocity; - btVector3 m_linearVelocity; - - btVector3 m_deltaLinearVelocity; - btVector3 m_deltaAngularVelocity; - - btVector3 m_centerOfMassPosition; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - - float m_angularFactor; - float m_invMass; - float m_friction; - btRigidBody* m_originalBody; - - - SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + SIMD_FORCE_INLINE btSimdScalar() { - velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE btSimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) { - //if (m_invMass) - { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - m_linearVelocity += linearComponent*impulseMagnitude; - m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } + } + + SIMD_FORCE_INLINE btSimdScalar(__m128 v128) + :m_vec128(v128) + { + } + union + { + __m128 m_vec128; + float m_floats[4]; + int m_ints[4]; + }; + SIMD_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + SIMD_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE void set128(__m128 v128) + { + m_vec128 = v128; + } + + SIMD_FORCE_INLINE operator __m128() + { + return m_vec128; + } + SIMD_FORCE_INLINE operator const __m128() const + { + return m_vec128; } - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - //if (m_invMass) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } + SIMD_FORCE_INLINE operator float() const + { + return m_floats[0]; } - - void writebackVelocity() - { - if (m_invMass) - { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); - - //m_originalBody->setCompanionId(-1); - } - } - - - void writebackVelocity(btScalar timeStep) - { - if (m_invMass) - { - m_originalBody->setLinearVelocity(m_linearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - - //m_originalBody->setCompanionId(-1); - } - } - - void readVelocity() - { - if (m_invMass) - { - m_linearVelocity = m_originalBody->getLinearVelocity(); - m_angularVelocity = m_originalBody->getAngularVelocity(); - } - } - - - - }; + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator*(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); +} + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator+(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); +} + + #else +#define btSimdScalar btScalar +#endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ATTRIBUTE_ALIGNED16 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMatrix3x3 m_worldInvInertiaTensor; - - btVector3 m_angularVelocity1; - btVector3 m_linearVelocity1; - - btVector3 m_deltaAngularVelocity; btVector3 m_deltaLinearVelocity; - - btVector3 m_centerOfMassPosition; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - - float m_angularFactor; - float m_invMass; - float m_friction; + btVector3 m_deltaAngularVelocity; + btScalar m_angularFactor; + btScalar m_invMass; + btScalar m_friction; btRigidBody* m_originalBody; - - + btVector3 m_pushVelocity; + //btVector3 m_turnVelocity; - SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { - velocity = (m_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).cross(rel_pos); + if (m_originalBody) + velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const { - if (m_invMass) + if (m_originalBody) + angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + { + //if (m_invMass) { m_deltaLinearVelocity += linearComponent*impulseMagnitude; m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_invMass) - { - m_pushVelocity += linearComponent*impulseMagnitude; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } +/* + void writebackVelocity() { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - + */ - void writebackVelocity(btScalar timeStep) + void writebackVelocity(btScalar timeStep=0) { if (m_invMass) { - m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); - - //correct the position/orientation based on push/turn recovery - btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); //m_originalBody->setCompanionId(-1); } } - - void readVelocity() - { - if (m_invMass) - { - m_linearVelocity1 = m_originalBody->getLinearVelocity(); - m_angularVelocity1 = m_originalBody->getAngularVelocity(); - } - } - }; -#endif //USE_JAC #endif //BT_SOLVER_BODY_H diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 1ebf08df6..459e406db 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -21,28 +21,26 @@ class btRigidBody; #include "LinearMath/btMatrix3x3.h" #include "btJacobianEntry.h" +#include //#define NO_FRICTION_TANGENTIALS 1 +#include "btSolverBody.h" + ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); -#ifdef _USE_JACOBIAN - btJacobianEntry m_jac; -#endif + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal; - - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; - - btVector3 m_relpos2CrossNormal; - btVector3 m_angularComponentA; + btVector3 m_relpos2CrossNormal; + btVector3 m_angularComponentA; btVector3 m_angularComponentB; - mutable btScalar m_appliedPushImpulse; - mutable btScalar m_appliedImpulse; + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; int m_solverBodyIdA; int m_solverBodyIdB; @@ -51,16 +49,14 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint btScalar m_restitution; btScalar m_jacDiagABInv; btScalar m_penetration; - - - + int m_constraintType; int m_frictionIndex; void* m_originalContactPoint; - btScalar m_rhs; - btScalar m_cfm; - btScalar m_lowerLimit; - btScalar m_upperLimit; + btScalar m_rhs; + btScalar m_cfm; + btScalar m_lowerLimit; + btScalar m_upperLimit; enum btSolverConstraintType { @@ -69,9 +65,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint }; }; - - - +typedef btAlignedObjectArray btConstraintArray; #endif //BT_SOLVER_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index c50ec6ec5..d383239cb 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -18,6 +18,8 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" +#include "btSolverConstraint.h" +struct btSolverBody; enum btTypedConstraintType { @@ -55,13 +57,53 @@ public: btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); + struct btConstraintInfo1 { + int m_numConstraintRows,nub; + }; + + struct btConstraintInfo2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + btScalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + btScalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + btScalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + virtual void buildJacobian() = 0; - virtual void solveConstraint(btScalar timeStep) = 0; + virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) + { + } + virtual void getInfo1 (btConstraintInfo1* info)=0; + virtual void getInfo2 (btConstraintInfo2* info)=0; + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + + const btRigidBody& getRigidBodyA() const { return m_rbA; @@ -94,7 +136,7 @@ public: { m_userConstraintId = uid; } - + int getUserConstraintId() const { return m_userConstraintId; diff --git a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h index e853f1488..4e4728ca6 100644 --- a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h +++ b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h @@ -140,22 +140,22 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint } m_flags; // Linear parts, used by all constraints - btQuadWordStorage m_relPos1; - btQuadWordStorage m_relPos2; - btQuadWordStorage m_jacdiagABInv; //Jacobian inverse multiplied by gamma (damping) for each axis - btQuadWordStorage m_linearBias; //depth*tau/(dt*gamma) along each axis + btVector3 m_relPos1; + btVector3 m_relPos2; + btVector3 m_jacdiagABInv; //Jacobian inverse multiplied by gamma (damping) for each axis + btVector3 m_linearBias; //depth*tau/(dt*gamma) along each axis // Joint-specific parts union { struct { - btQuadWordStorage m_frameAinW[3]; - btQuadWordStorage m_frameBinW[3]; + btVector3 m_frameAinW[3]; + btVector3 m_frameBinW[3]; // For angular - btQuadWordStorage m_angJacdiagABInv; //1/j - btQuadWordStorage m_angularBias; //error/dt, in x/y. limit error*bias factor / (dt * relaxation factor) in z + btVector3 m_angJacdiagABInv; //1/j + btVector3 m_angularBias; //error/dt, in x/y. limit error*bias factor / (dt * relaxation factor) in z // For limit float m_limitAccumulatedImpulse; @@ -168,8 +168,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint struct { - btQuadWordStorage m_swingAxis; - btQuadWordStorage m_twistAxis; + btVector3 m_swingAxis; + btVector3 m_twistAxis; float m_swingError; float m_swingJacInv; diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h index b79438399..c657afd2b 100644 --- a/src/LinearMath/btQuadWord.h +++ b/src/LinearMath/btQuadWord.h @@ -24,13 +24,13 @@ subject to the following restrictions: #include #endif -/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. +/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. */ #ifndef USE_LIBSPE2 -ATTRIBUTE_ALIGNED16(class) btQuadWordStorage +ATTRIBUTE_ALIGNED16(class) btQuadWord #else -class btQuadWordStorage +class btQuadWord #endif { protected: @@ -45,15 +45,11 @@ public: { return mVec128; } +protected: #else //__CELLOS_LV2__ __SPU__ btScalar m_floats[4]; #endif //__CELLOS_LV2__ __SPU__ -}; - -/** @brief The btQuadWord is base-class for vectors, points */ -class btQuadWord : public btQuadWordStorage -{ public: @@ -134,11 +130,7 @@ class btQuadWord : public btQuadWordStorage // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) { } - /**@brief Copy constructor */ - SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) - { - *((btQuadWordStorage*)this) = q; - } + /**@brief Three argument constructor (zeros w) * @param x Value of x * @param y Value of y diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 4f1dc17ae..cc24b1ede 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btVector3.h" +#include "btQuadWord.h" /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ class btQuaternion : public btQuadWord { diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 28a0e55a6..10a5af108 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -17,47 +17,77 @@ subject to the following restrictions: #ifndef SIMD__VECTOR3_H #define SIMD__VECTOR3_H -#include "btQuadWord.h" +#include "btScalar.h" +#include "btScalar.h" +#include "btMinMax.h" +#include /**@brief btVector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers */ -class btVector3 : public btQuadWord { +ATTRIBUTE_ALIGNED16(class) btVector3 +{ public: + +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + btScalar m_floats[4]; + }; +public: + vec_float4 get128() const + { + return mVec128; + } +public: +#else //__CELLOS_LV2__ __SPU__ +#ifdef WIN32 + union { + __m128 mVec128; + btScalar m_floats[4]; + }; + SIMD_FORCE_INLINE __m128 get128() const + { + return mVec128; + } + SIMD_FORCE_INLINE void set128(__m128 v128) + { + mVec128 = v128; + } +#else + btScalar m_floats[4]; +#endif +#endif //__CELLOS_LV2__ __SPU__ + + public: + /**@brief No initialization constructor */ SIMD_FORCE_INLINE btVector3() {} - /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid) - * Note: Vector3 derives from btQuadWordStorage*/ - SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) - : btQuadWord(q) - { - } + /**@brief Constructor from scalars * @param x X value * @param y Y value * @param z Z value */ - SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) - :btQuadWord(x,y,z,btScalar(0.)) + SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) { + m_floats[0] = x; + m_floats[1] = y; + m_floats[2] = z; + m_floats[3] = btScalar(0.); } -// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) -// : btQuadWord(x,y,z,w) -// { -// } - /**@brief Add a vector to this one * @param The vector to add to this one */ SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) { - m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z(); + m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } @@ -66,14 +96,14 @@ public: * @param The vector to subtract */ SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) { - m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z(); + m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } /**@brief Scale the vector * @param s Scale factor */ SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) { - m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; + m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } @@ -89,7 +119,7 @@ public: * @param v The other vector in the dot product */ SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const { - return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z(); + return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } /**@brief Return the length of the vector squared */ @@ -148,30 +178,30 @@ public: SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const { return btVector3( - m_floats[1] * v.z() - m_floats[2] * v.y(), - m_floats[2] * v.x() - m_floats[0] * v.z(), - m_floats[0] * v.y() - m_floats[1] * v.x()); + m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], + m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], + m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); } SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const { - return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + - m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + - m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x()); + return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); } /**@brief Return the axis with the smallest value * Note return values are 0,1,2 for x, y, or z */ SIMD_FORCE_INLINE int minAxis() const { - return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2); + return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const { - return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t, - m_floats[1] + (v.y() - m_floats[1]) * t, - m_floats[2] + (v.z() - m_floats[2]) * t); + return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); } /**@brief Elementwise multiply this vector by the other * @param v The other vector */ SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) { - m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z(); + m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } - + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;}; + /**@brief Set the y value */ + SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;}; + /**@brief Set the z value */ + SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;}; + /**@brief Set the w value */ + SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;}; + /**@brief Return the x value */ + SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; } + /**@brief Return the y value */ + SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; } + /**@brief Return the z value */ + SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; } + /**@brief Return the w value */ + SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; } + + //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } + //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } + SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } + + SIMD_FORCE_INLINE bool operator==(const btVector3& other) const + { + return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); + } + + SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMax(const btVector3& other) + { + btSetMax(m_floats[0], other.m_floats[0]); + btSetMax(m_floats[1], other.m_floats[1]); + btSetMax(m_floats[2], other.m_floats[2]); + btSetMax(m_floats[3], other.w()); + } + /**@brief Set each element to the min of the current values and the values of another btVector3 + * @param other The other btVector3 to compare with + */ + SIMD_FORCE_INLINE void setMin(const btVector3& other) + { + btSetMin(m_floats[0], other.m_floats[0]); + btSetMin(m_floats[1], other.m_floats[1]); + btSetMin(m_floats[2], other.m_floats[2]); + btSetMin(m_floats[3], other.w()); + } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3] = 0.f; + } }; @@ -220,34 +316,34 @@ public: SIMD_FORCE_INLINE btVector3 operator+(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); + return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } /**@brief Return the elementwise product of two vectors */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); + return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } /**@brief Return the difference between two vectors */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); + return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } /**@brief Return the negative of the vector */ SIMD_FORCE_INLINE btVector3 operator-(const btVector3& v) { - return btVector3(-v.x(), -v.y(), -v.z()); + return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } /**@brief Return the vector scaled by s */ SIMD_FORCE_INLINE btVector3 operator*(const btVector3& v, const btScalar& s) { - return btVector3(v.x() * s, v.y() * s, v.z() * s); + return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } /**@brief Return the vector scaled by s */ @@ -269,7 +365,7 @@ operator/(const btVector3& v, const btScalar& s) SIMD_FORCE_INLINE btVector3 operator/(const btVector3& v1, const btVector3& v2) { - return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); + return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } /**@brief Return the dot product between two vectors */ @@ -325,11 +421,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) return v1.lerp(v2, t); } -/**@brief Test if each element of the vector is equivalent */ -SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) -{ - return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z(); -} + SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const { @@ -404,7 +496,7 @@ public: if (m_floats[2] > maxVal) { maxIndex = 2; - maxVal = m_floats[2]; + maxVal =m_floats[2]; } if (m_floats[3] > maxVal) { @@ -437,7 +529,7 @@ public: if (m_floats[2] < minVal) { minIndex = 2; - minVal = m_floats[2]; + minVal =m_floats[2]; } if (m_floats[3] < minVal) { @@ -455,6 +547,40 @@ public: return absolute4().maxAxis4(); } + + + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + + +/* void getValue(btScalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] =m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + { + m_floats[0]=x; + m_floats[1]=y; + m_floats[2]=z; + m_floats[3]=w; + } + + + + };