/**
* @brief
* Returns the current joint anchor position
*/
void RagdollJoint::GetCurrentAnchor(Vector3 &vPosition) const
{
const Joint *pJoint = static_cast<Joint*>(m_pJointHandler->GetElement());
if (pJoint)
pJoint->GetCurrentPivotPoint(vPosition);
// We need to calculate the current anchor by hand :(
else {
// Get the attached body
const RagdollBody *pAttachedBody = m_pParentRagdoll->GetBody(sAttached);
if (pAttachedBody && pAttachedBody->bEnabled) {
// Get the attached physics body
const Body *pBody = static_cast<Body*>(pAttachedBody->GetBody());
if (pBody) {
// Get initial physics body transform matrix
Matrix3x4 mInitTrans;
mInitTrans.FromQuatTrans(pAttachedBody->qRot, pAttachedBody->vPos);
// Get current physics body transform matrix
Matrix3x4 mTrans;
pBody->GetTransformMatrix(mTrans);
// Get the current joint anchor in local space
const Vector3 vLocalAnchor = mInitTrans.GetInverted()*vJointAnchor;
// Get the current joint anchor in world space
vPosition = vLocalAnchor;
vPosition *= mTrans;
}
}
}
}
//[-------------------------------------------------------]
//[ Public virtual PLPhysics::Joint functions ]
//[-------------------------------------------------------]
void JointUniversal::GetCurrentPivotPoint(Vector3 &vPosition) const
{
const PLPhysics::Body *pParentBody = GetParentBody();
if (pParentBody) {
// Get transform matrix
Quaternion qQ;
pParentBody->GetRotation(qQ);
Vector3 vPos;
pParentBody->GetPosition(vPos);
Matrix3x4 mTrans;
mTrans.FromQuatTrans(qQ, vPos);
// Get the current joint anchor in world space
vPosition = m_vLocalAnchor;
vPosition *= mTrans;
}
}
/**
* @brief
* Constructor
*/
JointUniversal::JointUniversal(PLPhysics::World &cWorld, PLPhysics::Body *pParentBody, PLPhysics::Body *pChildBody,
const Vector3 &vPivotPoint, const Vector3 &vPinDir1, const Vector3 &vPinDir2) :
PLPhysics::JointUniversal(cWorld, static_cast<World&>(cWorld).CreateJointImpl(), pParentBody, pChildBody, vPivotPoint, vPinDir1, vPinDir2)
{
// Deactivate the physics simulation if required
const bool bSimulationActive = cWorld.IsSimulationActive();
if (bSimulationActive)
cWorld.SetSimulationActive(false);
// Get the Newton physics world
Newton::NewtonWorld *pNewtonWorld = static_cast<World&>(cWorld).GetNewtonWorld();
// Flush assigned bodies (MUST be done before creating the joint!)
if (pParentBody)
static_cast<BodyImpl&>(pParentBody->GetBodyImpl()).Flush();
if (pChildBody)
static_cast<BodyImpl&>(pChildBody ->GetBodyImpl()).Flush();
// Get the Newton physics parent and child bodies
const Newton::NewtonBody *pNewtonParentBody = pParentBody ? static_cast<BodyImpl&>(pParentBody->GetBodyImpl()).GetNewtonBody() : nullptr;
const Newton::NewtonBody *pNewtonChildBody = pChildBody ? static_cast<BodyImpl&>(pChildBody ->GetBodyImpl()).GetNewtonBody() : nullptr;
// [TODO] ??
/*
if (pNewtonParentBody)
NewtonBodySetUserData(pNewtonParentBody, nullptr);
if (pNewtonChildBody)
NewtonBodySetUserData(pNewtonChildBody, nullptr);
*/
// Get body initial transform matrix
if (pParentBody) {
// Get transform matrix
Quaternion qQ;
pParentBody->GetRotation(qQ);
Vector3 vPos;
pParentBody->GetPosition(vPos);
Matrix3x4 mTrans;
mTrans.FromQuatTrans(qQ, vPos);
// And transform the initial joint anchor into the body object space
mTrans.Invert();
m_vLocalAnchor = mTrans*vPivotPoint;
}
// Create the Newton physics joint
Newton::NewtonJoint *pNewtonJoint = NewtonConstraintCreateUniversal(pNewtonWorld, m_vPivotPoint, m_vPinDir1,
m_vPinDir2, pNewtonChildBody, pNewtonParentBody);
// Set Newton universal callback function
NewtonUniversalSetUserCallback(pNewtonJoint, JointUserCallback);
// Initialize the Newton physics joint
static_cast<JointImpl&>(GetJointImpl()).InitializeNewtonJoint(*this, *pNewtonJoint);
// Reactivate the physics simulation if required
if (bSimulationActive)
cWorld.SetSimulationActive(bSimulationActive);
}
