// fixed, breakable joint
PxJoint* createBreakableFixed(PxPhysics* gPhysics, PxRigidActor* a0, const PxTransform& t0, PxRigidActor* a1, const PxTransform& t1)
{
PxFixedJoint* j = PxFixedJointCreate(*gPhysics, a0, t0, a1, t1);
j->setBreakForce(1000, 100000);
j->setConstraintFlag(PxConstraintFlag::eDRIVE_LIMITS_ARE_FORCES, true);
return j;
}
/**
@brief
@date 2013-12-19
*/
void CCreature::CreateJoint( CPhenotypeNode *parentNode, CPhenotypeNode *childNode, genotype_parser::SConnection *connect,
const PxVec3 &conPos )
{
RET(!childNode);
RET(!parentNode);
PxRigidDynamic* body = parentNode->m_pBody;
PxRigidDynamic *child = childNode->m_pBody;
genotype_parser::SConnection *joint = connect;
//PxVec3 dir0(joint->parentOrient.dir.x, joint->parentOrient.dir.y, joint->parentOrient.dir.z);
//PxVec3 dir1(joint->orient.dir.x, joint->orient.dir.y, joint->orient.dir.z);
//PxVec3 pos = conPos;
//// random position
//PxVec3 randPos(connect->randPos.x, connect->randPos.y, connect->randPos.z);
//pos += RandVec3(randPos, 1.f);
//// random orientation
//PxVec3 randOrient(connect->randOrient.x, connect->randOrient.y, connect->randOrient.z);
//if (!dir1.isZero())
//{
// dir1 += RandVec3(randOrient, 1.f);
// dir1.normalize();
//}
//PxTransform tm0 = (dir0.isZero())? PxTransform::createIdentity() : PxTransform(PxQuat(joint->parentOrient.angle, dir0));
//PxTransform tm1 = (dir1.isZero())? PxTransform(PxVec3(pos)) :
// (PxTransform(PxQuat(joint->orient.angle, dir1)) * PxTransform(PxVec3(pos)));
PxTransform tm0, tm1;
GetJointTransform(&conPos, joint, tm0, tm1);
PxVec3 limit = utility::Vec3toPxVec3(joint->limit);
PxVec3 velocity = utility::Vec3toPxVec3(joint->velocity);
//apply gravity direction only have root genotype
if (boost::iequals(parentNode->m_ShapeName, "root"))
{
PxVec3 gravDir = parentNode->m_pBody->getGlobalPose().p;
gravDir.normalize();
gravDir = PxVec3(0,1,0);
PxQuat gravQ;
utility::quatRotationArc(gravQ, PxVec3(0,1,0), gravDir);
tm1 = tm1 * PxTransform(gravQ);
}
PxJoint* pxJoint = NULL;
const PxReal tolerance = 0.2f;
if (boost::iequals(joint->type, "fixed"))
{
PxFixedJoint *j = PxFixedJointCreate(m_sample.getPhysics(), body, tm0, child, tm1);
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
else if (boost::iequals(joint->type, "spherical"))
{
if (PxSphericalJoint *j = PxSphericalJointCreate(m_sample.getPhysics(), body, tm0, child, tm1) )
{
if (!limit.isZero())
{
j->setLimitCone(PxJointLimitCone(limit.x, limit.y, PxSpring(0,0)));
j->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
}
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
}
else if (boost::iequals(joint->type, "revolute"))
{
if (PxRevoluteJoint*j = PxRevoluteJointCreate(m_sample.getPhysics(), body, tm0, child, tm1) )
{
if (!limit.isZero())
{
j->setLimit(PxJointAngularLimitPair(limit.x, limit.y, limit.z)); // upper, lower, tolerance
j->setRevoluteJointFlag(PxRevoluteJointFlag::eLIMIT_ENABLED, true);
}
if (!velocity.isZero())
{
j->setDriveVelocity(velocity.x);
j->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_ENABLED, true);
}
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
}
CJoint *pJoint = new CJoint(parentNode, childNode, tm0, tm1, pxJoint, velocity.x, joint->period);
CAngularSensor *pSensor = new CAngularSensor();
CMuscleEffector *pEffector = new CMuscleEffector(joint->period);
pJoint->ApplySensor(*pSensor);
pJoint->ApplyEffector(*pEffector);
parentNode->m_Joints.push_back(pJoint);
parentNode->m_Sensors.push_back(pSensor);
parentNode->m_Effectors.push_back(pEffector);
{ // add child
CAngularSensor *pChildSensor = new CAngularSensor();
pJoint->ApplySensor(*pChildSensor);
childNode->m_pParentJointSensor = pChildSensor;
}
}