/******************************************************************************/ #include "stdafx.h" namespace EE{ /******************************************************************************/ enum JOINT_TYPE : Byte { JOINT_NONE , JOINT_FIXED , JOINT_HINGE , JOINT_SPHERICAL, JOINT_SLIDER , JOINT_DISTANCE , }; /******************************************************************************/ #if PHYSX #define BOUNCE 0.1f #define DAMPING 0.5f*0 #define SPRING 1.0f*0 /*struct SphericalJoint : PxJoint, PxConstraintConnector { static const PxU32 TYPE_ID = PxConcreteType::eFIRST_USER_EXTENSION; // PxJoint virtual void setActors(PxRigidActor* actor0, PxRigidActor* actor1)override {} virtual void getActors(PxRigidActor*& actor0, PxRigidActor*& actor1)const override {} virtual void setLocalPose(PxJointActorIndex::Enum actor, const PxTransform& localPose)override {} virtual PxTransform getLocalPose(PxJointActorIndex::Enum actor)const override {} virtual PxTransform getRelativeTransform()const override {} virtual PxVec3 getRelativeLinearVelocity()const override {} virtual PxVec3 getRelativeAngularVelocity()const override {} virtual void setBreakForce(PxReal force, PxReal torque)override {} virtual void getBreakForce(PxReal& force, PxReal& torque)const override {} virtual void setConstraintFlags(PxConstraintFlags flags)override {} virtual void setConstraintFlag(PxConstraintFlag::Enum flag, bool value)override {} virtual PxConstraintFlags getConstraintFlags()const override {} virtual void setInvMassScale0(PxReal invMassScale)override {} virtual PxReal getInvMassScale0()const override {} virtual void setInvInertiaScale0(PxReal invInertiaScale) override {} virtual PxReal getInvInertiaScale0()const override {} virtual void setInvMassScale1(PxReal invMassScale)override {} virtual PxReal getInvMassScale1()const override {} virtual void setInvInertiaScale1(PxReal invInertiaScale)override {} virtual PxReal getInvInertiaScale1()const override {} virtual PxConstraint* getConstraint()const override {} virtual void setName(const char* name)override {} virtual const char* getName()const override {} virtual void release()override {} virtual PxScene* getScene()const override {} // PxConstraintConnector virtual void* prepareData()override {} virtual bool updatePvdProperties(physx::pvdsdk::PvdDataStream& pvdConnection, const PxConstraint* c, PxPvdUpdateType::Enum updateType)const override {} virtual void onConstraintRelease()override {} virtual void onComShift(PxU32 actor)override {} virtual void onOriginShift(const PxVec3& shift)override {} virtual void* getExternalReference(PxU32& typeID)override {} virtual PxBase* getSerializable()override {} virtual PxConstraintSolverPrep getPrep()const override {} virtual const void* getConstantBlock()const override {} };*/ #else struct btDistanceConstraint : btPoint2PointConstraint { Bool spring; Flt min_dist, max_dist, spring_spring, spring_damper; btDistanceConstraint(btRigidBody& rbA, const btVector3& pivotInA, Flt min_dist, Flt max_dist, Bool spring, Flt spring_spring, Flt spring_damper) : btPoint2PointConstraint(rbA, pivotInA) { T.min_dist =min_dist ; T.max_dist =max_dist ; T.spring =spring ; T.spring_spring=spring_spring; T.spring_damper=spring_damper; } btDistanceConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB, Flt min_dist, Flt max_dist, Bool spring, Flt spring_spring, Flt spring_damper) : btPoint2PointConstraint(rbA, rbB, pivotInA, pivotInB) { T.min_dist =min_dist ; T.max_dist =max_dist ; T.spring =spring ; T.spring_spring=spring_spring; T.spring_damper=spring_damper; } virtual void getInfo1(btConstraintInfo1 *info) { info->m_numConstraintRows=1; info->nub=5; } virtual void getInfo2(btConstraintInfo2 *info) { btVector3 relA = m_rbA.getCenterOfMassTransform().getBasis() * getPivotInA(); btVector3 relB = m_rbB.getCenterOfMassTransform().getBasis() * getPivotInB(); btVector3 posA = m_rbA.getCenterOfMassTransform().getOrigin() + relA; btVector3 posB = m_rbB.getCenterOfMassTransform().getOrigin() + relB; btVector3 del = posB - posA; btScalar currDist = btSqrt(del.dot(del)); btVector3 ortho = (currDist ? del / currDist : btVector3(0, 0, 0)); info->m_J1linearAxis[0] = ortho[0]; info->m_J1linearAxis[1] = ortho[1]; info->m_J1linearAxis[2] = ortho[2]; btVector3 p, q; p = relA.cross(ortho); q = relB.cross(ortho); info->m_J1angularAxis[0] = p[0]; info->m_J1angularAxis[1] = p[1]; info->m_J1angularAxis[2] = p[2]; info->m_J2angularAxis[0] = -q[0]; info->m_J2angularAxis[1] = -q[1]; info->m_J2angularAxis[2] = -q[2]; btScalar rhs = (currDist - 0/*m_distance*/) * info->fps * info->erp * (spring ? spring_spring*0.01f : 1); info->m_constraintError[0] = rhs; info->cfm[0] = btScalar(0.f); info->m_lowerLimit[0] = -SIMD_INFINITY; info->m_upperLimit[0] = SIMD_INFINITY; } }; #endif /******************************************************************************/ Joint& Joint::del() { if(_joint) { SafeWriteLock lock(Physics._rws); if(_joint) { #if PHYSX if(Physx.world)_joint->release(); _joint=null; #else if(_joint->needsFeedback())Bullet.breakables.exclude(_joint); // remove from breakable list if(Bullet.world)Bullet.world->removeConstraint(_joint); // remove from world Delete(_joint); #endif } } return T; } /******************************************************************************/ static Bool ValidActors(Actor &a0, Actor *a1) // PhysX will still create joint for null actors, and creating for static only will even create a crash { return a0.is() && (a1 ? a1->is() : true) // make sure that both exist #if PHYSX && (a0.isDynamic() || (a1 && a1->isDynamic())) // for PhysX make sure that at least one is dynamic #endif ; } /******************************************************************************/ Joint& Joint::create(Actor &a0, Actor *a1) { WriteLock lock(Physics._rws); del(); #if PHYSX if(Physx.world && ValidActors(a0, a1)) if(PxFixedJoint *fixed=PxFixedJointCreate(*Physx.physics, a0._actor, PxTransform(PxIdentity), a1 ? a1->_actor : null, Physx.matrix(a1 ? a0.matrix().divNormalized(a1->matrix()) : a0.matrix()))) // when specifying world matrix use current a0 world position, when specifying local frame for a1 use current a0 position in a1 space { fixed->setConstraintFlag(PxConstraintFlag::eCOLLISION_ENABLED, false); _joint=fixed; } #else if(a0._actor) { btGeneric6DofConstraint *dof=null; if(a1 && a1->_actor) { dof=new btGeneric6DofConstraint(*a0._actor, *a1->_actor, btTransform::getIdentity(), Bullet.matrix(a0.massCenterMatrix()/a1->massCenterMatrix()), false); }else { dof=new btGeneric6DofConstraint(*a0._actor, btTransform::getIdentity(), false); } dof->setAngularLowerLimit(btVector3(0, 0, 0)); dof->setAngularUpperLimit(btVector3(0, 0, 0)); dof->setLinearLowerLimit (btVector3(0, 0, 0)); dof->setLinearUpperLimit (btVector3(0, 0, 0)); if(_joint=dof) { Flt f=FLT_MAX; _joint->setUserConstraintId((Int&)f); _joint->setUserConstraintType(0); if(Bullet.world)Bullet.world->addConstraint(_joint, true); } } #endif return T; } /******************************************************************************/ static void CreateHinge(Joint &joint, Actor &a0, Actor *a1, C Vec local_anchor[2], C Vec local_axis[2], C Vec local_normal[2], Bool limit_angle, Bool body, Flt angle_min, Flt angle_max, Bool collision) { #if PHYSX Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.y=local_normal[0]; m0.z=Cross(m0.x, m0.y); Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.y=local_normal[1]; m1.z=Cross(m1.x, m1.y); WriteLock lock(Physics._rws); if(Physx.world && ValidActors(a0, a1)) if(PxRevoluteJoint *hinge=PxRevoluteJointCreate(*Physx.physics, a0._actor, Physx.matrix(m0), a1 ? a1->_actor : null, Physx.matrix(m1))) { hinge->setConstraintFlag (PxConstraintFlag ::eCOLLISION_ENABLED, collision ); hinge->setRevoluteJointFlag(PxRevoluteJointFlag::eLIMIT_ENABLED , limit_angle); PxJointAngularLimitPair limit(-angle_max, -angle_min); if(body){limit.restitution=BOUNCE; limit.stiffness=SPRING; limit.damping=DAMPING;} hinge->setLimit(limit); joint._joint=hinge; } #else if(a0._actor) { Matrix m0; m0.pos=local_anchor[0]; m0.z=local_axis[0]; m0.x=local_normal[0]; m0.y=Cross(m0.z, m0.x); m0*=a0. _actor->offset; Matrix m1; m1.pos=local_anchor[1]; m1.z=local_axis[1]; m1.x=local_normal[1]; m1.y=Cross(m1.z, m1.x); if(a1 && a1->_actor)m1*=a1->_actor->offset; btHingeConstraint *hinge=null; if(a1 && a1->_actor) { hinge=new btHingeConstraint(*a0._actor, *a1->_actor, Bullet.matrix(m0), Bullet.matrix(m1)); }else { hinge=new btHingeConstraint(*a0._actor, Bullet.matrix(m0)); hinge->getBFrame()=Bullet.matrix(m1); } if(joint._joint=hinge) { Flt f=FLT_MAX; hinge->setUserConstraintId((Int&)f); hinge->setUserConstraintType(0); if(limit_angle)hinge->setLimit(angle_min, angle_max); WriteLock lock(Physics._rws); if(Bullet.world)Bullet.world->addConstraint(hinge, !collision); } } #endif } /******************************************************************************/ static void CreateSpherical(Joint &joint, Actor &a0, Actor *a1, C Vec local_anchor[2], C Vec local_axis[2], C Vec local_normal[2], Bool collision, Bool limit_swing, Bool limit_twist, Bool body, Flt swing, Flt twist) { #if PHYSX #if 0 // PxSphericalJoint Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.y=local_normal[0]; m0.z=Cross(m0.x, m0.y); Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.y=local_normal[1]; m1.z=Cross(m1.x, m1.y); WriteLock lock(Physics._rws); if(Physx.world && ValidActors(a0, a1)) if(PxSphericalJoint *spherical=PxSphericalJointCreate(*Physx.physics, a0._actor, Physx.matrix(m0), a1 ? a1->_actor : null, Physx.matrix(m1))) { spherical->setConstraintFlag (PxConstraintFlag ::eCOLLISION_ENABLED , collision ); spherical->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED , limit_swing); spherical->setSphericalJointFlag(PxSphericalJointFlag::eTWIST_LIMIT_ENABLED, limit_twist); Clamp(swing, EPS, PI-EPS); Clamp(twist, EPS, PI-EPS); PxJointLimitCone limit(swing, swing); limit.twist=twist; if(body){limit.restitution=BOUNCE; limit.stiffness=SPRING; limit.damping=DAMPING;} spherical->setLimitCone(limit); joint._joint=spherical; } #else // PxD6Joint Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.y=local_normal[0]; m0.z=Cross(m0.x, m0.y); Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.y=local_normal[1]; m1.z=Cross(m1.x, m1.y); WriteLock lock(Physics._rws); if(Physx.world && ValidActors(a0, a1)) if(PxD6Joint *spherical=PxD6JointCreate(*Physx.physics, a0._actor, Physx.matrix(m0), a1 ? a1->_actor : null, Physx.matrix(m1))) { spherical->setConstraintFlag(PxConstraintFlag::eCOLLISION_ENABLED , collision ); spherical->setMotion (PxD6Axis::eSWING1, limit_swing ? PxD6Motion::eLIMITED : PxD6Motion::eFREE); spherical->setMotion (PxD6Axis::eSWING2, limit_swing ? PxD6Motion::eLIMITED : PxD6Motion::eFREE); spherical->setMotion (PxD6Axis::eTWIST , limit_twist ? PxD6Motion::eLIMITED : PxD6Motion::eFREE); if(limit_swing) { Clamp(swing, EPS, PI-EPS); PxJointLimitCone limit(swing, swing); if(body){limit.restitution=BOUNCE; limit.stiffness=SPRING; limit.damping=DAMPING;} spherical->setSwingLimit(limit); } if(limit_twist) { Clamp(twist, 0, PI); PxJointAngularLimitPair limit(-twist, twist); if(body){limit.restitution=BOUNCE; limit.stiffness=SPRING; limit.damping=DAMPING;} spherical->setTwistLimit(limit); } joint._joint=spherical; } #endif #else if(a0._actor) { if(!limit_swing && !limit_twist) { btPoint2PointConstraint *p2p=null; if(a1 && a1->_actor) { p2p=new btPoint2PointConstraint(*a0._actor, *a1->_actor, Bullet.vec(local_anchor[0]*a0._actor->offset), Bullet.vec(local_anchor[1]*a1->_actor->offset)); }else { p2p=new btPoint2PointConstraint(*a0._actor, Bullet.vec(local_anchor[0]*a0._actor->offset)); p2p->setPivotB(Bullet.vec(local_anchor[1])); } if(joint._joint=p2p) { Flt f=FLT_MAX; p2p->setUserConstraintId((Int&)f); p2p->setUserConstraintType(0); WriteLock lock(Physics._rws); if(Bullet.world)Bullet.world->addConstraint(p2p, !collision); } }else { Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.z=local_normal[0]; m0.y=Cross(m0.z, m0.x); m0*=a0. _actor->offset; Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.z=local_normal[1]; m1.y=Cross(m1.z, m1.x); if(a1 && a1->_actor)m1*=a1->_actor->offset; btConeTwistConstraint *cone=null; if(a1 && a1->_actor) { cone=new btConeTwistConstraint(*a0._actor, *a1->_actor, Bullet.matrix(m0), Bullet.matrix(m1)); }else { cone=new btConeTwistConstraint(*a0._actor, Bullet.matrix(m0)); cone->getBFrame()=Bullet.matrix(m1); } if(joint._joint=cone) { Flt f=FLT_MAX; cone->setUserConstraintId((Int&)f); cone->setUserConstraintType(0); cone->setLimit(limit_swing ? swing : FLT_MAX, limit_swing ? swing : FLT_MAX, limit_twist ? twist : FLT_MAX); WriteLock lock(Physics._rws); if(Bullet.world)Bullet.world->addConstraint(cone, !collision); } } } #endif } /******************************************************************************/ static void CreateSlider(Joint &joint, Actor &a0, Actor *a1, C Vec local_anchor[2], C Vec local_axis[2], C Vec local_normal[2], Flt min, Flt max, Bool collision) { #if PHYSX Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.y=local_normal[0]; m0.z=Cross(m0.x, m0.y); Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.y=local_normal[1]; m1.z=Cross(m1.x, m1.y); WriteLock lock(Physics._rws); if(Physx.world && ValidActors(a0, a1)) if(PxPrismaticJoint *slider=PxPrismaticJointCreate(*Physx.physics, a0._actor, Physx.matrix(m0), a1 ? a1->_actor : null, Physx.matrix(m1))) { slider->setConstraintFlag (PxConstraintFlag ::eCOLLISION_ENABLED, collision); slider->setPrismaticJointFlag(PxPrismaticJointFlag::eLIMIT_ENABLED , true ); PxJointLinearLimitPair limit(Physx.physics->getTolerancesScale(), -max, -min); slider->setLimit(limit); joint._joint=slider; } #else if(a0._actor) { Matrix m0; m0.pos=local_anchor[0]; m0.x=local_axis[0]; m0.z=local_normal[0]; m0.y=Cross(m0.z, m0.x); m0*=a0. _actor->offset; Matrix m1; m1.pos=local_anchor[1]; m1.x=local_axis[1]; m1.z=local_normal[1]; m1.y=Cross(m1.z, m1.x); if(a1 && a1->_actor)m1*=a1->_actor->offset; btSliderConstraint *slider=null; if(a1 && a1->_actor) { slider=new btSliderConstraint(*a1->_actor, *a0._actor, Bullet.matrix(m1), Bullet.matrix(m0), true); // btSliderConstraint(bodyA, bodyB), because constructor below accepts B, to match order we need to swap here actor order }else { slider=new btSliderConstraint(*a0._actor, Bullet.matrix(m0), true); // btSliderConstraint(bodyB), this accepts B only slider->setFrames(Bullet.matrix(m1), Bullet.matrix(m0)); } if(joint._joint=slider) { Flt f=FLT_MAX; slider->setUserConstraintId((Int&)f); slider->setUserConstraintType(0); slider->setLowerLinLimit(min); slider->setUpperLinLimit(max); WriteLock lock(Physics._rws); if(Bullet.world)Bullet.world->addConstraint(slider, !collision); } } #endif } /******************************************************************************/ static void CreateDistance(Joint &joint, Actor &a0, Actor *a1, C Vec local_anchor[2], Flt min, Flt max, Bool collision, Bool spring, Flt spring_spring, Flt spring_damper) { #if PHYSX WriteLock lock(Physics._rws); if(Physx.world && ValidActors(a0, a1)) if(PxDistanceJoint *distance=PxDistanceJointCreate(*Physx.physics, a0._actor, Physx.matrix(local_anchor[0]), a1 ? a1->_actor : null, Physx.matrix(local_anchor[1]))) { distance->setConstraintFlag (PxConstraintFlag::eCOLLISION_ENABLED, collision); distance->setMinDistance (min); distance->setMaxDistance (max); distance->setStiffness (spring_spring); distance->setDamping (spring_damper); distance->setDistanceJointFlag(PxDistanceJointFlag::eMIN_DISTANCE_ENABLED, true ); distance->setDistanceJointFlag(PxDistanceJointFlag::eMAX_DISTANCE_ENABLED, true ); distance->setDistanceJointFlag(PxDistanceJointFlag::eSPRING_ENABLED , spring); joint._joint=distance; } #else if(a0._actor) { btDistanceConstraint *dist=null; if(a1 && a1->_actor) { dist=new btDistanceConstraint(*a0._actor, *a1->_actor, Bullet.vec(local_anchor[0]*a0._actor->offset), Bullet.vec(local_anchor[1]*a1->_actor->offset), min, max, spring, spring_spring, spring_damper); }else { dist=new btDistanceConstraint(*a0._actor, Bullet.vec(local_anchor[0]*a0._actor->offset), min, max, spring, spring_spring, spring_damper); dist->setPivotB(Bullet.vec(local_anchor[1])); } if(joint._joint=dist) { Flt f=FLT_MAX; dist->setUserConstraintId((Int&)f); dist->setUserConstraintType(0); WriteLock lock(Physics._rws); if(Bullet.world)Bullet.world->addConstraint(dist, !collision); } } #endif } /******************************************************************************/ Joint& Joint::createHinge(Actor &a0, Actor *a1, C Vec &anchor, C Vec &axis, Bool collision) { del(); Matrix m0 = a0. matrix(), m1 =(a1 ? a1->matrix() : MatrixIdentity); Vec normal =PerpN(axis), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(axis ).divNormalized(m0.orn()), Vec(axis ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateHinge(T, a0, a1, local_anchor, local_axis, local_normal, false, false, 0, 0, collision); return T; } Joint& Joint::createHinge(Actor &a0, Actor *a1, C Vec &anchor, C Vec &axis, Flt min_angle, Flt max_angle, Bool collision) { del(); Matrix m0 = a0. matrix(), m1 =(a1 ? a1->matrix() : MatrixIdentity); Vec normal =PerpN(axis), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(axis ).divNormalized(m0.orn()), Vec(axis ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateHinge(T, a0, a1, local_anchor, local_axis, local_normal, true, false, min_angle, max_angle, collision); return T; } Joint& Joint::createSpherical(Actor &a0, Actor *a1, C Vec &anchor, C Vec &axis, Flt *swing, Flt *twist, Bool collision) { del(); Matrix m0 = a0. matrix(), m1 =(a1 ? a1->matrix() : MatrixIdentity); Vec normal =PerpN(axis), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(axis ).divNormalized(m0.orn()), Vec(axis ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateSpherical(T, a0, a1, local_anchor, local_axis, local_normal, collision, swing!=null, twist!=null, false, swing ? *swing : 0, twist ? *twist : 0); return T; } Joint& Joint::createSliding(Actor &a0, Actor *a1, C Vec &anchor, C Vec &dir, Flt min, Flt max, Bool collision) { del(); Matrix m0 = a0. matrix(), m1 =(a1 ? a1->matrix() : MatrixIdentity); Vec normal =PerpN(dir), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(dir ).divNormalized(m0.orn()), Vec(dir ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateSlider(T, a0, a1, local_anchor, local_axis, local_normal, min, max, collision); return T; } Joint& Joint::createDist(Actor &a0, Actor *a1, C Vec &anchor0, C Vec &anchor1, Flt min, Flt max, Spring *spring, Bool collision) { del(); Vec local_anchor[]={anchor0, anchor1}; CreateDistance(T, a0, a1, local_anchor, min, max, collision, spring!=null, spring ? spring->spring : 0, spring ? spring->damping : 0); return T; } Joint& Joint::createBodyHinge(Actor &bone, Actor &parent, C Vec &anchor, C Vec &axis, Flt min_angle, Flt max_angle) { del(); Matrix m0 =bone .matrix(), m1 =parent.matrix(); Vec normal =PerpN(axis), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(axis ).divNormalized(m0.orn()), Vec(axis ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateHinge(T, bone, &parent, local_anchor, local_axis, local_normal, true, true, min_angle, max_angle, false); return T; } Joint& Joint::createBodySpherical(Actor &bone, Actor &parent, C Vec &anchor, C Vec &axis, Flt swing, Flt twist) { del(); Matrix m0 =bone .matrix(), m1 =parent.matrix(); Vec normal =PerpN(axis), local_anchor[]={Vec(anchor).divNormalized(m0 ), Vec(anchor).divNormalized(m1 )}, local_axis []={Vec(axis ).divNormalized(m0.orn()), Vec(axis ).divNormalized(m1.orn())}, local_normal[]={Vec(normal).divNormalized(m0.orn()), Vec(normal).divNormalized(m1.orn())}; CreateSpherical(T, bone, &parent, local_anchor, local_axis, local_normal, false, true, true, true, swing, twist); return T; } /******************************************************************************/ Matrix Joint::localAnchor(Bool index)C { #if PHYSX if(_joint)return Physx.matrix(_joint->getLocalPose(index ? PxJointActorIndex::eACTOR1 : PxJointActorIndex::eACTOR0)); #endif return MatrixIdentity; } void Joint::changed() { #if PHYSX if(_joint)if(PxConstraint *constraint=_joint->getConstraint())constraint->markDirty(); #endif } /******************************************************************************/ // BREAKABLE /******************************************************************************/ Bool Joint::broken()C { #if PHYSX return _joint ? FlagTest((UInt)_joint->getConstraintFlags(), PxConstraintFlag::eBROKEN) : true; #else return _joint ? _joint->getUserConstraintType()!=0 : true; #endif } /******************************************************************************/ Joint& Joint::breakable(Flt max_force, Flt max_torque) { if(_joint) { #if PHYSX _joint->setBreakForce((max_force <0) ? PX_MAX_REAL : max_force , (max_torque<0) ? PX_MAX_REAL : max_torque); #else Flt f; if(max_force >=FLT_MAX)max_force =-1; // need to adjust to -1 because depending on <0 we include/exclude in breakable list if(max_torque>=FLT_MAX)max_torque=-1; // need to adjust to -1 because depending on <0 we include/exclude in breakable list if(max_force>=0 || max_torque>=0) { Bullet.breakables.include(_joint); // add to breakable list _joint->enableFeedback(true); if(max_force>=0 && max_torque>=0)f=Avg(max_force, max_torque);else if(max_force>=0 )f= max_force ;else if( max_torque>=0)f= max_torque ; }else { Bullet.breakables.exclude(_joint); // remove from breakable list _joint->enableFeedback(false); f=FLT_MAX; } _joint->setUserConstraintId((Int&)f); #endif } return T; } /******************************************************************************/ // HINGE /******************************************************************************/ Flt Joint::hingeAngle()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return hinge->getAngle(); #endif return 0; } Flt Joint::hingeVel()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return hinge->getVelocity(); #endif return 0; } Bool Joint::hingeDriveEnabled()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return FlagTest((UInt)hinge->getRevoluteJointFlags(), PxRevoluteJointFlag::eDRIVE_ENABLED); #endif return false; } Joint& Joint::hingeDriveEnabled(Bool on) { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())hinge->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_ENABLED, on); #endif return T; } Bool Joint::hingeDriveFreeSpin()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return FlagTest((UInt)hinge->getRevoluteJointFlags(), PxRevoluteJointFlag::eDRIVE_FREESPIN); #endif return false; } Joint& Joint::hingeDriveFreeSpin(Bool on) { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())hinge->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_FREESPIN, on); #endif return T; } Flt Joint::hingeDriveVel()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return hinge->getDriveVelocity(); #endif return 0; } Joint& Joint::hingeDriveVel(Flt vel) { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())hinge->setDriveVelocity(vel); #endif return T; } Flt Joint::hingeDriveForceLimit()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return hinge->getDriveForceLimit(); #endif return 0; } Joint& Joint::hingeDriveForceLimit(Flt limit) { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())hinge->setDriveForceLimit(limit); #endif return T; } Flt Joint::hingeDriveGearRatio()C { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())return hinge->getDriveGearRatio(); #endif return 0; } Joint& Joint::hingeDriveGearRatio(Flt ratio) { #if PHYSX if(_joint)if(PxRevoluteJoint *hinge=_joint->is())hinge->setDriveGearRatio(ratio); #endif return T; } /******************************************************************************/ // IO /******************************************************************************/ Bool Joint::save(File &f)C { f.cmpUIntV(0); // version JOINT_TYPE type=JOINT_NONE; if(_joint && !broken()) { #if PHYSX switch(_joint->getConcreteType()) { case PxJointConcreteType::eFIXED : type=JOINT_FIXED ; break; case PxJointConcreteType::eREVOLUTE : type=JOINT_HINGE ; break; case PxJointConcreteType::ePRISMATIC: type=JOINT_SLIDER ; break; case PxJointConcreteType::eDISTANCE : type=JOINT_DISTANCE ; break; case PxJointConcreteType::eD6 : case PxJointConcreteType::eSPHERICAL: type=JOINT_SPHERICAL; break; } #else if(CAST(btDistanceConstraint , _joint))type=JOINT_DISTANCE ;else // this must be checked as first, because 'btDistanceConstraint' extends 'btPoint2PointConstraint' checked below if(CAST(btGeneric6DofConstraint, _joint))type=JOINT_FIXED ;else if(CAST(btHingeConstraint , _joint))type=JOINT_HINGE ;else if(CAST(btPoint2PointConstraint, _joint))type=JOINT_SPHERICAL;else if(CAST(btConeTwistConstraint , _joint))type=JOINT_SPHERICAL;else if(CAST(btSliderConstraint , _joint))type=JOINT_SLIDER ; #endif } // save joint type f.putByte(type); // save joint data switch(type) { case JOINT_HINGE: { Vec anchor[2], axis[2], normal[2]; Bool collision, limit_angle, body; Flt angle_min, angle_max; #if PHYSX PxRevoluteJoint &hinge=*_joint->is(); Matrix m0=Physx.matrix(hinge.getLocalPose(PxJointActorIndex::eACTOR0)), m1=Physx.matrix(hinge.getLocalPose(PxJointActorIndex::eACTOR1)); PxJointAngularLimitPair limit=hinge.getLimit(); anchor[0]=m0.pos; anchor[1]=m1.pos; axis [0]=m0.x; axis [1]=m1.x; normal[0]=m0.y; normal[1]=m1.y; collision =FlagTest((UInt)hinge.getConstraintFlags (), PxConstraintFlag ::eCOLLISION_ENABLED); limit_angle=FlagTest((UInt)hinge.getRevoluteJointFlags(), PxRevoluteJointFlag::eLIMIT_ENABLED ); body =(limit.restitution>0 || limit.stiffness>0); angle_min =-limit.upper; angle_max =-limit.lower; #else btHingeConstraint *hinge=CAST(btHingeConstraint, _joint); Matrix m=Bullet.matrix(hinge->getAFrame()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))m.divNormalized(rb->offset); anchor[0]=m.pos; axis[0]=m.z; normal[0]=m.y; m=Bullet.matrix(hinge->getBFrame()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyB()))m.divNormalized(rb->offset); anchor[1]=m.pos; axis[1]=m.z; normal[1]=m.y; collision=true; btRigidBody &rb=_joint->getRigidBodyA(); REP(rb.getNumConstraintRefs())if(rb.getConstraintRef(i)==_joint){collision=false; break;} body =false; angle_min=hinge->getLowerLimit(); angle_max=hinge->getUpperLimit(); limit_angle=(angle_min<=angle_max); #endif f<is(); Matrix m0=Physx.matrix(spherical.getLocalPose(PxJointActorIndex::eACTOR0)), m1=Physx.matrix(spherical.getLocalPose(PxJointActorIndex::eACTOR1)); PxJointLimitCone limit=spherical.getLimitCone(); anchor[0]=m0.pos; anchor[1]=m1.pos; axis [0]=m0.x; axis [1]=m1.x; normal[0]=m0.y; normal[1]=m1.y; collision =FlagTest((UInt)spherical.getConstraintFlags (), PxConstraintFlag ::eCOLLISION_ENABLED ); limit_swing=FlagTest((UInt)spherical.getSphericalJointFlags(), PxSphericalJointFlag::eLIMIT_ENABLED ); limit_twist=FlagTest((UInt)spherical.getSphericalJointFlags(), PxSphericalJointFlag::eTWIST_LIMIT_ENABLED); body =(limit.restitution>0 || limit.stiffness>0); swing =limit.yAngle; twist =limit.twistAngle; #else PxD6Joint &spherical=*_joint->is(); PxJointLimitCone swing_limit=spherical.getSwingLimit(); PxJointAngularLimitPair twist_limit=spherical.getTwistLimit(); Matrix m0=Physx.matrix(spherical.getLocalPose(PxJointActorIndex::eACTOR0)), m1=Physx.matrix(spherical.getLocalPose(PxJointActorIndex::eACTOR1)); anchor[0]=m0.pos; anchor[1]=m1.pos; axis [0]=m0.x; axis [1]=m1.x; normal[0]=m0.y; normal[1]=m1.y; collision =FlagTest((UInt)spherical.getConstraintFlags(), PxConstraintFlag::eCOLLISION_ENABLED); limit_swing=(spherical.getMotion(PxD6Axis::eSWING1)!=PxD6Motion::eFREE); limit_twist=(spherical.getMotion(PxD6Axis::eTWIST )!=PxD6Motion::eFREE); body =(swing_limit.restitution>0 || swing_limit.stiffness>0); swing = swing_limit.yAngle; twist = twist_limit.upper; #endif #else if(btPoint2PointConstraint *p2p=CAST(btPoint2PointConstraint, _joint)) { anchor[0]=Bullet.vec(p2p->getPivotInA()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))anchor[0].divNormalized(rb->offset); axis[0].set(0, 0, 1); normal[0].set(1, 0, 0); anchor[1]=Bullet.vec(p2p->getPivotInB()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyB()))anchor[1].divNormalized(rb->offset); axis[1].set(0, 0, 1); normal[1].set(1, 0, 0); limit_swing=limit_twist=false; }else { btConeTwistConstraint *cone=CAST(btConeTwistConstraint, _joint); Matrix m=Bullet.matrix(cone->getAFrame()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))m.divNormalized(rb->offset); anchor[0]=m.pos; axis[0]=m.x; normal[0]=m.z; m=Bullet.matrix(cone->getBFrame()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyB()))m.divNormalized(rb->offset); anchor[1]=m.pos; axis[1]=m.x; normal[1]=m.z; swing=cone->getSwingSpan1(); limit_swing=(swing!=FLT_MAX); twist=cone->getTwistSpan (); limit_twist=(twist!=FLT_MAX); } collision=true; btRigidBody &rb=_joint->getRigidBodyA(); REP(rb.getNumConstraintRefs())if(rb.getConstraintRef(i)==_joint){collision=false; break;} body =false; #endif f<is(); Matrix m0=Physx.matrix(slider.getLocalPose(PxJointActorIndex::eACTOR0)), m1=Physx.matrix(slider.getLocalPose(PxJointActorIndex::eACTOR1)); PxJointLinearLimitPair limit=slider.getLimit(); anchor[0]=m0.pos; anchor[1]=m1.pos; axis [0]=m0.x; axis [1]=m1.x; normal[0]=m0.y; normal[1]=m1.y; collision=FlagTest((UInt)slider.getConstraintFlags(), PxConstraintFlag::eCOLLISION_ENABLED); min =-limit.upper; max =-limit.lower; #else btSliderConstraint *slider=CAST(btSliderConstraint, _joint); Matrix m=Bullet.matrix(slider->getFrameOffsetA()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))m.divNormalized(rb->offset); anchor[1]=m.pos; axis[1]=m.x; normal[1]=m.z; m=Bullet.matrix(slider->getFrameOffsetB()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyB()))m.divNormalized(rb->offset); anchor[0]=m.pos; axis[0]=m.x; normal[0]=m.z; collision=true; btRigidBody &rb=_joint->getRigidBodyA(); REP(rb.getNumConstraintRefs())if(rb.getConstraintRef(i)==_joint){collision=false; break;} min=slider->getLowerLinLimit(); max=slider->getUpperLinLimit(); #endif f<is(); anchor[0] =Physx.vec(distance.getLocalPose(PxJointActorIndex::eACTOR0).p); anchor[1] =Physx.vec(distance.getLocalPose(PxJointActorIndex::eACTOR1).p); collision =FlagTest((UInt)distance.getConstraintFlags (), PxConstraintFlag ::eCOLLISION_ENABLED); spring =FlagTest((UInt)distance.getDistanceJointFlags(), PxDistanceJointFlag::eSPRING_ENABLED ); min =distance.getMinDistance(); max =distance.getMaxDistance(); spring_spring=distance.getStiffness(); spring_damper=distance.getDamping (); #else btDistanceConstraint *dist=CAST(btDistanceConstraint, _joint); anchor[0] =Bullet.vec(dist->getPivotInA()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))anchor[0].divNormalized(rb->offset); anchor[1] =Bullet.vec(dist->getPivotInB()); if(RigidBody *rb=CAST(RigidBody, &_joint->getRigidBodyA()))anchor[1].divNormalized(rb->offset); min =dist->min_dist; max =dist->max_dist; spring =dist->spring; spring_spring=dist->spring_spring; spring_damper=dist->spring_damper; collision=true; btRigidBody &rb=_joint->getRigidBodyA(); REP(rb.getNumConstraintRefs())if(rb.getConstraintRef(i)==_joint){collision=false; break;} #endif f<getBreakForce(max_force, max_torque); #else Int force=_joint->getUserConstraintId(); max_force=max_torque=(Flt&)force; #endif f<>anchor>>axis>>normal>>collision>>limit_angle>>body; if(limit_angle)f>>angle_min>>angle_max; CreateHinge(T, a0, a1, anchor, axis, normal, limit_angle, body, angle_min, angle_max, collision); hingeDriveEnabled(f.getBool()); hingeDriveFreeSpin(f.getBool()); hingeDriveVel(f.getFlt()); hingeDriveForceLimit(f.getFlt()); hingeDriveGearRatio(f.getFlt()); }break; case JOINT_SPHERICAL: { Vec anchor[2], axis[2], normal[2]; Bool collision, limit_swing, limit_twist, body; Flt swing, twist; f>>anchor>>axis>>normal>>collision>>limit_swing>>limit_twist>>body; if(limit_swing)f>>swing; if(limit_twist)f>>twist; CreateSpherical(T, a0, a1, anchor, axis, normal, collision, limit_swing, limit_twist, body, swing, twist); }break; case JOINT_SLIDER: { Vec anchor[2], axis[2], normal[2]; Flt min, max; Bool collision; f>>anchor>>axis>>normal>>min>>max>>collision; CreateSlider(T, a0, a1, anchor, axis, normal, min, max, collision); }break; case JOINT_DISTANCE: { Vec anchor[2]; Flt min, max; Bool collision, spring; Flt spring_spring, spring_damper; f>>anchor>>min>>max>>collision>>spring; if(spring)f>>spring_spring>>spring_damper; CreateDistance(T, a0, a1, anchor, min, max, collision, spring, spring_spring, spring_damper); }break; } // load breakable data if(type) { Flt max_force, max_torque; f>>max_force>>max_torque; breakable(max_force, max_torque); } if(f.ok())return true; }break; } error: del(); return false; } /******************************************************************************/ } /******************************************************************************/