From b8cbfe5f72f3ffb0a4cb4e5605292413c4b17e8f Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Fri, 28 Jul 2006 21:49:26 +0000 Subject: [PATCH] Basic support for COLLADA physics constraints (each DOF can be completely locked or free, no limits yet) --- .../Generic6DofConstraint.cpp | 158 +++++++------ .../ConstraintSolver/Generic6DofConstraint.h | 50 +++- Demos/ColladaDemo/ColladaDemo.cpp | 219 ++++++++++++++---- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 56 +++++ .../CcdPhysics/CcdPhysicsEnvironment.h | 16 ++ 5 files changed, 379 insertions(+), 120 deletions(-) diff --git a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp index 0059b10e8..9597f2721 100644 --- a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp +++ b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp @@ -32,6 +32,10 @@ Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, con , m_frameInA(frameInA) , m_frameInB(frameInB) { + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //so start all locked for (int i=0; i<6;++i) { m_lowerLimit[i] = 0.0f; @@ -59,50 +63,56 @@ void Generic6DofConstraint::BuildJacobian() //linear part for (i=0;i<3;i++) { - normal[i] = 1; - - // Create linear atom - new (&m_jac[i]) JacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); + if (isLimited(i)) + { + normal[i] = 1; + + // Create linear atom + new (&m_jacLinear[i]) JacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); - // Apply accumulated impulse - SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal; + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal; - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); - normal[i] = 0; + normal[i] = 0; + } } // angular part for (i=0;i<3;i++) { - SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + if (isLimited(i+3)) + { + SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe - SimdVector3 axis = kSign[i] * axisA.cross(axisB); + // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe + SimdVector3 axis = kSign[i] * axisA.cross(axisB); - // Create angular atom - new (&m_jacAng[i]) JacobianEntry(axis, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); + // Create angular atom + new (&m_jacAng[i]) JacobianEntry(axis, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); - // Apply accumulated impulse - SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; + // Apply accumulated impulse + SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } } } @@ -123,58 +133,64 @@ void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep) // linear for (i=0;i<3;i++) { - SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - - normal[i] = 1; - SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); - - //velocity error (first order error) - SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); + if (isLimited(i)) + { + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; - m_accumulatedImpulse[i] += impulse; - SimdVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse( impulse_vector, rel_pos1); - m_rbB.applyImpulse(-impulse_vector, rel_pos2); + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + + //velocity error (first order error) + SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); - normal[i] = 0; + //positional error (zeroth order error) + SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i] += impulse; + + SimdVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } } // angular for (i=0;i<3;i++) { - SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + if (isLimited(i+3)) + { + SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - //velocity error (first order error) - SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); + SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + + //velocity error (first order error) + SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); - //positional error (zeroth order error) - SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); - SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + //positional error (zeroth order error) + SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); - SimdScalar rel_pos = kSign[i] * axisA.dot(axisB); + SimdScalar rel_pos = kSign[i] * axisA.dot(axisB); - //impulse - SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; - m_accumulatedImpulse[i + 3] += impulse; - - // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) - SimdVector3 axis = kSign[i] * axisA.cross(axisB); - SimdVector3 impulse_vector = axis * impulse; + //impulse + SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i + 3] += impulse; + + // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) + SimdVector3 axis = kSign[i] * axisA.cross(axisB); + SimdVector3 impulse_vector = axis * impulse; - m_rbA.applyTorqueImpulse( impulse_vector); - m_rbB.applyTorqueImpulse(-impulse_vector); + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } } } diff --git a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h index f63fa5af3..7a7725350 100644 --- a/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h +++ b/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h @@ -30,7 +30,7 @@ class RigidBody; /// Work in progress (is still a Hinge actually) class Generic6DofConstraint : public TypedConstraint { - JacobianEntry m_jac[3]; // 3 orthogonal linear constraints + JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints SimdTransform m_frameInA; // the constraint space w.r.t body A @@ -55,11 +55,49 @@ public: SimdScalar ComputeAngle(int axis) const; - void SetAngularLimit(int axis, SimdScalar lo, SimdScalar hi) - { - m_lowerLimit[axis + 3] = lo; - m_upperLimit[axis + 3] = hi; - } + void setLinearLowerLimit(const SimdVector3& linearLower) + { + m_lowerLimit[0] = linearLower.getX(); + m_lowerLimit[1] = linearLower.getY(); + m_lowerLimit[2] = linearLower.getZ(); + } + + void setLinearUpperLimit(const SimdVector3& linearUpper) + { + m_upperLimit[0] = linearUpper.getX(); + m_upperLimit[1] = linearUpper.getY(); + m_upperLimit[2] = linearUpper.getZ(); + } + + void setAngularLowerLimit(const SimdVector3& angularLower) + { + m_lowerLimit[3] = angularLower.getX(); + m_lowerLimit[4] = angularLower.getY(); + m_lowerLimit[5] = angularLower.getZ(); + } + + void setAngularUpperLimit(const SimdVector3& angularUpper) + { + m_upperLimit[3] = angularUpper.getX(); + m_upperLimit[4] = angularUpper.getY(); + m_upperLimit[5] = angularUpper.getZ(); + } + + //first 3 are linear, next 3 are angular + void SetLimit(int axis, SimdScalar lo, SimdScalar hi) + { + m_lowerLimit[axis] = lo; + m_upperLimit[axis] = hi; + } + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } const RigidBody& GetRigidBodyA() const { diff --git a/Demos/ColladaDemo/ColladaDemo.cpp b/Demos/ColladaDemo/ColladaDemo.cpp index ca51c36b8..c9d99fd25 100644 --- a/Demos/ColladaDemo/ColladaDemo.cpp +++ b/Demos/ColladaDemo/ColladaDemo.cpp @@ -87,9 +87,57 @@ extern int gForwardAxis; #include "dae.h" #include "dom/domCOLLADA.h" + + + DAE* collada = 0; domCOLLADA* dom = 0; + +domMatrix_Array emptyMatrixArray; + +SimdTransform GetSimdTransformFromCOLLADA_DOM(domMatrix_Array& matrixArray, + domRotate_Array& rotateArray, + domTranslate_Array& translateArray + ) + +{ + SimdTransform startTransform; + startTransform.setIdentity(); + + int i; + //either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate' + for (i=0;igetValue(); + SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11)); + startTransform.setOrigin(origin); + SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2), + fl16.get(4),fl16.get(5),fl16.get(6), + fl16.get(8),fl16.get(9),fl16.get(10)); + startTransform.setBasis(basis); + } + + for (i=0;igetValue(); + float angleRad = SIMD_RADS_PER_DEG*fl4.get(3); + SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad); + startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat); + } + + for (i=0;igetValue(); + startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2)); + } + return startTransform; +} + + #endif @@ -160,7 +208,7 @@ CollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use of //////////////////////////////////// ///Very basic import -void CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape) +CcdPhysicsController* CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startTransform,CollisionShape* shape) { startTransforms[numObjects] = startTransform; @@ -245,8 +293,8 @@ void CreatePhysicsObject(bool isDynamic, float mass, const SimdTransform& startT } - numObjects++; - + //return newly created PhysicsController + return physObjects[numObjects++]; } @@ -1449,38 +1497,12 @@ int main(int argc,char** argv) //find transform of the node that this rigidbody maps to - int i; - //either load the matrix (worldspace) or incrementally build the transform from 'translate'/'rotate' - for (i=0;igetMatrix_array().getCount();i++) - { - domMatrixRef matrixRef = node->getMatrix_array()[i]; - domFloat4x4 fl16 = matrixRef->getValue(); - SimdVector3 origin(fl16.get(3),fl16.get(7),fl16.get(11)); - startTransform.setOrigin(origin); - SimdMatrix3x3 basis(fl16.get(0),fl16.get(1),fl16.get(2), - fl16.get(4),fl16.get(5),fl16.get(6), - fl16.get(8),fl16.get(9),fl16.get(10)); - startTransform.setBasis(basis); - } - - - - - for (i=0;igetRotate_array().getCount();i++) - { - domRotateRef rotateRef = node->getRotate_array()[i]; - domFloat4 fl4 = rotateRef->getValue(); - float angleRad = SIMD_RADS_PER_DEG*fl4.get(3); - SimdQuaternion rotQuat(SimdVector3(fl4.get(0),fl4.get(1),fl4.get(2)),angleRad); - startTransform.getBasis() = startTransform.getBasis() * SimdMatrix3x3(rotQuat); - } - - for (i=0;igetTranslate_array().getCount();i++) - { - domTranslateRef translateRef = node->getTranslate_array()[i]; - domFloat3 fl3 = translateRef->getValue(); - startTransform.getOrigin() += SimdVector3(fl3.get(0),fl3.get(1),fl3.get(2)); - } + + startTransform = GetSimdTransformFromCOLLADA_DOM( + node->getMatrix_array(), + node->getRotate_array(), + node->getTranslate_array() + ); for (i=0;igetScale_array().getCount();i++) { @@ -1493,7 +1515,9 @@ int main(int argc,char** argv) - CreatePhysicsObject(isDynamics,mass,startTransform,colShape); + CcdPhysicsController* ctrl = CreatePhysicsObject(isDynamics,mass,startTransform,colShape); + //for bodyName lookup in constraints + ctrl->setNewClientInfo((void*)bodyName); } @@ -1527,19 +1551,128 @@ int main(int argc,char** argv) if (rigidConstraintRef->getSid() && !strcmp(rigidConstraintRef->getSid(),constraintName)) { - /* + //two bodies - rigidConstraintRef->getRef_attachment(); - rigidConstraintRef->getAttachment(); + const domRigid_constraint::domRef_attachmentRef attachRefBody = rigidConstraintRef->getRef_attachment(); + const domRigid_constraint::domAttachmentRef attachBody1 = rigidConstraintRef->getAttachment(); + + daeString uri = attachRefBody->getRigid_body().getURI(); + daeString orgUri0 = attachRefBody->getRigid_body().getOriginalURI(); + daeString orgUri1 = attachBody1->getRigid_body().getOriginalURI(); + CcdPhysicsController* ctrl0=0,*ctrl1=0; + + for (int i=0;igetNewClientInfo(); + if (!strcmp(bodyName,orgUri0)) + { + printf("found\n"); + ctrl0=physObjects[i]; + } + if (!strcmp(bodyName,orgUri1)) + { + ctrl1=physObjects[i]; + } + } + + + + const domRigid_constraint::domAttachmentRef attachOtherBody = rigidConstraintRef->getAttachment(); const domRigid_constraint::domTechnique_commonRef commonRef = rigidConstraintRef->getTechnique_common(); - domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue(); - SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2)); + domFloat3 flMin = commonRef->getLimits()->getLinear()->getMin()->getValue(); SimdVector3 minLinearLimit(flMin.get(0),flMin.get(1),flMin.get(2)); - commonRef->getLimits()->getSwing_cone_and_twist(); -*/ + + domFloat3 flMax = commonRef->getLimits()->getLinear()->getMax()->getValue(); + SimdVector3 maxLinearLimit(flMax.get(0),flMax.get(1),flMax.get(2)); + + domFloat3 coneMinLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMin()->getValue(); + SimdVector3 angularMin(coneMinLimit.get(0),coneMinLimit.get(1),coneMinLimit.get(2)); + + domFloat3 coneMaxLimit = commonRef->getLimits()->getSwing_cone_and_twist()->getMax()->getValue(); + SimdVector3 angularMax(coneMaxLimit.get(0),coneMaxLimit.get(1),coneMaxLimit.get(2)); + + { + int constraintId; + + SimdTransform attachFrameRef0; + attachFrameRef0 = + GetSimdTransformFromCOLLADA_DOM + ( + emptyMatrixArray, + attachRefBody->getRotate_array(), + attachRefBody->getTranslate_array()); + + SimdTransform attachFrameOther; + attachFrameOther = + GetSimdTransformFromCOLLADA_DOM + ( + emptyMatrixArray, + attachBody1->getRotate_array(), + attachBody1->getTranslate_array() + ); + + + //convert INF / -INF into lower > upper + + //currently there is a hack in the DOM to detect INF / -INF + //see daeMetaAttribute.cpp + //INF -> 999999.9 + //-INF -> -999999.9 + float linearCheckTreshold = 999999.0; + float angularCheckTreshold = 180.0;//check this + + + + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + + SimdVector3 linearLowerLimits = minLinearLimit; + SimdVector3 linearUpperLimits = maxLinearLimit; + SimdVector3 angularLowerLimits = angularMin; + SimdVector3 angularUpperLimits = angularMax; + { + for (int i=0;i<3;i++) + { + if ((linearLowerLimits[i] < -linearCheckTreshold) || + (linearUpperLimits[i] > linearCheckTreshold)) + { + //disable limits + linearLowerLimits[i] = 1; + linearUpperLimits[i] = 0; + } + + if ((angularLowerLimits[i] < -angularCheckTreshold) || + (angularUpperLimits[i] > angularCheckTreshold)) + { + //disable limits + angularLowerLimits[i] = 1; + angularUpperLimits[i] = 0; + } + } + } + + + constraintId =physicsEnvironmentPtr->createUniversalD6Constraint( + ctrl0, + ctrl1, + attachFrameRef0, + attachFrameOther, + linearLowerLimits, + linearUpperLimits, + angularLowerLimits, + angularUpperLimits + ); + + + } + + } } diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index e8bfd38d5..dc94e809e 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -1327,6 +1327,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl } + + + +//Following the COLLADA physics specification for constraints +int CcdPhysicsEnvironment::createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& frameInA, + SimdTransform& frameInB, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits +) +{ + + //we could either add some logic to recognize ball-socket and hinge, or let that up to the user + //perhaps some warning or hint that hinge/ball-socket is more efficient? + + Generic6DofConstraint* genericConstraint = 0; + CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef; + CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther; + + RigidBody* rb0 = ctrl0->GetRigidBody(); + RigidBody* rb1 = ctrl1->GetRigidBody(); + + if (rb1) + { + + + genericConstraint = new Generic6DofConstraint( + *rb0,*rb1, + frameInA,frameInB); + genericConstraint->setLinearLowerLimit(linearMinLimits); + genericConstraint->setLinearUpperLimit(linearMaxLimits); + genericConstraint->setAngularLowerLimit(angularMinLimits); + genericConstraint->setAngularUpperLimit(angularMaxLimits); + } else + { + // TODO: Implement single body case... + //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code + + } + + if (genericConstraint) + { + m_constraints.push_back(genericConstraint); + genericConstraint->SetUserConstraintId(gConstraintUid++); + genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT); + //64 bit systems can't cast pointer to int. could use size_t instead. + return genericConstraint->GetUserConstraintId(); + } + return 0; +} + + + void CcdPhysicsEnvironment::removeConstraint(int constraintId) { std::vector::iterator i; diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 3bde70f31..3078f51f8 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -20,6 +20,8 @@ subject to the following restrictions: #include class CcdPhysicsController; #include "SimdVector3.h" +#include "SimdTransform.h" + @@ -119,6 +121,20 @@ protected: virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type, float pivotX,float pivotY,float pivotZ, float axisX,float axisY,float axisZ); + + + //Following the COLLADA physics specification for constraints + virtual int createUniversalD6Constraint( + class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther, + SimdTransform& localAttachmentFrameRef, + SimdTransform& localAttachmentOther, + const SimdVector3& linearMinLimits, + const SimdVector3& linearMaxLimits, + const SimdVector3& angularMinLimits, + const SimdVector3& angularMaxLimits + ); + + virtual void removeConstraint(int constraintid);