Changeset 8284 for code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

Timestamp:

Apr 21, 2011, 6:58:23 PM (14 years ago)

Author:

rgrieder

Message:

Merged revisions 7978 - 8096 from kicklib to kicklib2.

Location:

code/branches/kicklib2

Files:

• . (modified) (1 prop)
• src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp (modified) (9 diffs)

code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp

@@ -21 +21 @@
 
 
-btPoint2PointConstraint::btPoint2PointConstraint()
-: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),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
 {
@@ -37 +34 @@
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
 :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_flags(0),
 m_useSolveConstraintObsolete(false)
 {
@@ -44 +42 @@
 void    btPoint2PointConstraint::buildJacobian()
 {
+
         ///we need it for both methods
         {
@@ -67 +66 @@
         }
 
-}
-
+
+}
 
 void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
+{
+        getInfo1NonVirtual(info);
+}
+
+void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
 {
         if (m_useSolveConstraintObsolete)
@@ -83 +87 @@
 }
 
+
+
+
 void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
 {
+        getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
+}
+
+void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
+{
         btAssert(!m_useSolveConstraintObsolete);
 
          //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.
@@ -97 +105 @@
     // set jacobian
     info->m_J1linearAxis[0] = 1;
-    info->m_J1linearAxis[info->rowskip+1] = 1;
-    info->m_J1linearAxis[2*info->rowskip+2] = 1;
+        info->m_J1linearAxis[info->rowskip+1] = 1;
+        info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
         btVector3 a1 = body0_trans.getBasis()*getPivotInA();
@@ -127 +135 @@
 
     // set right hand side
-    btScalar k = info->fps * info->erp;
+        btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
+    btScalar k = info->fps * currERP;
     int j;
-
         for (j=0; j<3; j++)
     {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = 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]);
     }
+        if(m_flags & BT_P2P_FLAGS_CFM)
+        {
+                for (j=0; j<3; j++)
+                {
+                        info->cfm[j*info->rowskip] = m_cfm;
+                }
+        }
 
         btScalar impulseClamp = m_setting.m_impulseClamp;//
@@ -145 +160 @@
                 }
         }
-        
-}
-
-
-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);
-                
-
-        //      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 deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
-                        btScalar impulseClamp = m_setting.m_impulseClamp;
-                        
-                        const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
-                        if (sum < -impulseClamp)
-                        {
-                                deltaImpulse = -impulseClamp-m_appliedImpulse;
-                                m_appliedImpulse = -impulseClamp;
-                        }
-                        else if (sum > impulseClamp) 
-                        {
-                                deltaImpulse = impulseClamp-m_appliedImpulse;
-                                m_appliedImpulse = impulseClamp;
-                        }
-                        else
-                        {
-                                m_appliedImpulse = sum;
-                        }
-
-                        
-                        btVector3 impulse_vector = normal * deltaImpulse;
-                        
-                        btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
-                        btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-                        bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
-                        bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
-
-
-                        normal[i] = 0;
-                }
-        }
-}
+        info->m_damping = m_setting.m_damping;
+        
+}
+
+
 
 void    btPoint2PointConstraint::updateRHS(btScalar     timeStep)
@@ -229 +172 @@
 }
 
+///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+///If no axis is provided, it uses the default axis for this constraint.
+void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
+{
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                m_erp = value; 
+                                m_flags |= BT_P2P_FLAGS_ERP;
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                m_cfm = value; 
+                                m_flags |= BT_P2P_FLAGS_CFM;
+                                break;
+                        default: 
+                                btAssertConstrParams(0);
+                }
+        }
+}
+
+///return the local value of parameter
+btScalar btPoint2PointConstraint::getParam(int num, int axis) const 
+{
+        btScalar retVal(SIMD_INFINITY);
+        if(axis != -1)
+        {
+                btAssertConstrParams(0);
+        }
+        else
+        {
+                switch(num)
+                {
+                        case BT_CONSTRAINT_ERP :
+                        case BT_CONSTRAINT_STOP_ERP :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
+                                retVal = m_erp; 
+                                break;
+                        case BT_CONSTRAINT_CFM :
+                        case BT_CONSTRAINT_STOP_CFM :
+                                btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
+                                retVal = m_cfm; 
+                                break;
+                        default: 
+                                btAssertConstrParams(0);
+                }
+        }
+        return retVal;
+}
+