void Sc::BodySim::calculateKinematicVelocity(PxReal oneOverDt)
{
PX_ASSERT(isKinematic());
/*------------------------------------------------\
| kinematic bodies are moved directly by the user and are not influenced by external forces
| we simply determine the distance moved since the last simulation frame and
| assign the appropriate delta to the velocity. This vel will be used to shove dynamic
| objects in the solver.
| We have to do this like so in a delayed way, because when the user sets the target pos the dt is not
| yet known.
\------------------------------------------------*/
PX_ASSERT(isActive());
BodyCore& core = getBodyCore();
if (readInternalFlag(BF_KINEMATIC_MOVED))
{
clearInternalFlag(BF_KINEMATIC_SETTLING);
const SimStateData* kData = core.getSimStateData(true);
PX_ASSERT(kData);
PX_ASSERT(kData->isKine());
PX_ASSERT(kData->getKinematicData()->targetValid);
PxVec3 linVelLL, angVelLL;
PxTransform targetPose = kData->getKinematicData()->targetPose;
const PxTransform& currBody2World = getBody2World();
//the kinematic target pose is now the target of the body (CoM) and not the actor.
PxVec3 deltaPos = targetPose.p;
deltaPos -= currBody2World.p;
linVelLL = deltaPos * oneOverDt;
PxQuat q = targetPose.q * currBody2World.q.getConjugate();
if (q.w < 0) //shortest angle.
q = -q;
PxReal angle;
PxVec3 axis;
q.toRadiansAndUnitAxis(angle, axis);
angVelLL = axis * angle * oneOverDt;
core.setLinearVelocity(linVelLL);
core.setAngularVelocity(angVelLL);
// Moving a kinematic should trigger a wakeUp call on a higher level.
PX_ASSERT(core.getWakeCounter()>0);
PX_ASSERT(isActive());
}
else
{
core.setLinearVelocity(PxVec3(0));
core.setAngularVelocity(PxVec3(0));
}
}
/**
@brief MouseRButtonUp
@date 2014-03-04
*/
void COrientationEditController::MouseRButtonUp(physx::PxU32 x, physx::PxU32 y)
{
switch (m_editMode)
{
case MODE_POSITION:
{
RET(!m_selectNode);
using namespace genotype_parser;
SConnection *joint = m_rootNode->GetJoint(m_selectNode);
RET(!joint);
PxVec3 dir = m_selectNode->GetPos() - m_rootNode->GetPos();
const float len = dir.magnitude();
dir.normalize();
PxQuat rotateToXAxis;
if (boost::iequals(joint->type, "revolute"))
{
PxVec3 jointAxis = PxVec3(0,0,1).cross(dir);
jointAxis.normalize();
utility::quatRotationArc(rotateToXAxis, PxVec3(1,0,0), jointAxis);
}
else
{
rotateToXAxis = m_rootNode->GetWorldTransform().q;
}
{ // setting parent orientation
PxReal angle;
PxVec3 axis;
rotateToXAxis.toRadiansAndUnitAxis(angle, axis);
joint->parentOrient.angle = angle;
joint->parentOrient.dir = utility::PxVec3toVec3(axis);
printf( "parent angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
}
{ // setting select orientation
PxVec3 pos;
PxTransform rotTm;
const PxTransform tm = m_selectNode->GetWorldTransform();
if (boost::iequals(joint->type, "revolute"))
{
rotTm = PxTransform(rotateToXAxis) * PxTransform(tm.q);
pos = rotTm.rotate(dir*len);
}
else
{
rotTm = PxTransform(tm.q);
pos = tm.p;
}
PxReal angle;
PxVec3 axis;
rotTm.q.toRadiansAndUnitAxis(angle, axis);
joint->orient.angle = angle;
joint->orient.dir = utility::PxVec3toVec3(axis);
joint->pos = utility::PxVec3toVec3(pos);
printf( "connect angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z);
}
SelectNode(NULL);
}
break;
}
if (m_cursor)
m_cursor->MouseRButtonUp(x,y);
}