C++ Matrix3x4::FromQuatTrans方法代码示例

/**
*  @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);
}