// 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;
}
/**
* Method is used to add joint between two scene actors. This joints can be broken when defined
* amount of force/troque have been applied.
* @param name is first actor name id.
* @param name2 is second actor name id.
* @param force is amount of force needed to brake joint.
* @param torque is amount of torque needed to brake joint.
*/
void PhysicsManager::addJoint(const string& name, const string& name2, const float force, const float torque)
{
DynamicActor* first = nullptr;
DynamicActor* second = nullptr;
DynamicActors::const_iterator it = dynamicActors.begin();
for(; it != dynamicActors.end(); ++it)
if((*it)->entityLogic->entityName == name)
break;
if(it == dynamicActors.end())
return;
first = (*it);
it = dynamicActors.begin();
for(; it != dynamicActors.end(); ++it)
if((*it)->entityLogic->entityName == name2)
break;
if(it == dynamicActors.end())
return;
second = (*it);
PxTransform transformation;
PxVec3 position = PxVec3(first->entityLogic->entityState.position[0],first->entityLogic->entityState.position[1],first->entityLogic->entityState.position[2]);
PxQuat orientation = PxQuat(first->entityLogic->entityState.orientation[0],first->entityLogic->entityState.orientation[1],first->entityLogic->entityState.orientation[2],first->entityLogic->entityState.orientation[3]);
transformation.p = position;
transformation.q = orientation;
PxTransform transformation2;
PxVec3 position2 = PxVec3(second->entityLogic->entityState.position[0],second->entityLogic->entityState.position[1],second->entityLogic->entityState.position[2]);
PxQuat orientation2 = PxQuat(second->entityLogic->entityState.orientation[0],second->entityLogic->entityState.orientation[1],second->entityLogic->entityState.orientation[2],second->entityLogic->entityState.orientation[3]);
transformation2.p = position2;
transformation2.q = orientation2;
PxFixedJoint* fixed = PxFixedJointCreate(*physicsSDK,first->entityPhysics,transformation,second->entityPhysics,transformation2);
fixed->setBreakForce(force,torque);
}
/**
@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;
}
}
