Vector3 StillDesign::PhysX::Math::QuatToEuler( NxQuat q )
{
q.normalize();
float sqw = q.w * q.w;
float sqx = q.x * q.x;
float sqy = q.y * q.y;
float sqz = q.z * q.z;
float test = q.x*q.y + q.z*q.w;
Vector3 rotation = Vector3( 0, 0, 0 );
// Singularity at north pole
if( test >= 0.499999999999999999f )
{
rotation.Y = 2.0f * atan2( q.x, q.w );
rotation.Z = (float)System::Math::PI;
rotation.X = 0.0f;
return rotation;
}
if( test <= -0.499999999999999999f )
{
rotation.Y = -2.0f * atan2( q.x, q.w );
rotation.Z = -((float)System::Math::PI / 2.0f);
rotation.X = 0.0f;
return rotation;
}
rotation.Y = atan2(2*q.y*q.w-2*q.x*q.z , sqx - sqy - sqz + sqw);
rotation.Z = asin(2*test);
rotation.X = atan2(2*q.x*q.w-2*q.y*q.z , -sqx + sqy - sqz + sqw);
return rotation;
}
void TriPod::attach(IAgriDevice *device)
{
if(m_attachedTrailer)
return;
if(m_attachedDevice)
return;
m_attachedDevice = device;
Vec3 transformedDeviceConnectorTop;
Vec3 transformedDeviceConnectorBottom;
Vec3 transformedTriPodPos;
Vec3 diff = m_triPodDimms->posTopPodDeviceConnector - m_triPodDimms->posTopPodTractorConnector;
Vec3 transDiff;
D3DXVec3TransformCoord(&transDiff, &diff, &m_matTop);
if(m_frontPod)
{
D3DXVec3TransformCoord(&transformedDeviceConnectorTop, &m_triPodDimms->posTopPodTractorConnector, &m_matTop);
D3DXVec3TransformCoord(&transformedDeviceConnectorBottom, &m_triPodDimms->posBottomPodTractorConnector, &m_matBottom);
}
else
{
D3DXVec3TransformCoord(&transformedDeviceConnectorTop, &m_triPodDimms->posTopPodDeviceConnector, &m_matTop);
D3DXVec3TransformCoord(&transformedDeviceConnectorBottom, &m_triPodDimms->posBottomPodDeviceConnector, &m_matBottom);
}
NxMat34 pose = m_vehicle->getVehicleController()->getActor()->getGlobalPose();
if(m_frontPod)
{
Mat mat;
D3DXMatrixRotationY(&mat, D3DX_PI);
NxMat33 mat33;
NxQuat quat;
pose.M.toQuat(quat);
quat.normalize();
NxReal angle;
NxVec3 axis;
quat.getAngleAxis(angle, axis);
axis.x *= -1;
axis.z *= -1;
//quat.w *= -1;
quat.fromAngleAxis(angle, axis);
pose.M.fromQuat(quat);
pose = NxMat34(NxMat33(NxVec3(mat._11, mat._12, mat._13), NxVec3(mat._21, mat._22, mat._23), NxVec3(mat._31, mat._32, mat._33)), NxVec3(0, 0, 0)) * pose;
}
NxVec3 trans = NxVec3(m_vehicle->getVehicleController()->getForwardVec() * -10);
pose.t.x = pose.t.x + trans.x;
pose.t.y = pose.t.y + trans.y;
pose.t.z = pose.t.z + trans.z;
m_attachedDevice->getActor()->setGlobalPose(pose);
m_attachedDevice->update();
transformedTriPodPos = transformedDeviceConnectorTop - m_triPodDimms->posTopPod;
Vec3 move = transformedTriPodPos - m_attachedDevice->getTriPodPos();
m_attachedDevice->getActor()->setGlobalPosition(m_attachedDevice->getActor()->getGlobalPosition() + NxVec3(move));
m_attachedDevice->update();
//NxRevoluteJointDesc revDescTop;
//revDescTop.actor[0] = m_actorTop;
//revDescTop.actor[1] = m_attachedDevice->getActor();
//revDescTop.localNormal[0] = NxVec3(0, 1, 0);
//revDescTop.localNormal[1] = NxVec3(0, 1, 0);
//revDescTop.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescTop.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posTopPod)); //Reference point that the axis passes through.
//m_jointTopPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescTop);
Vec3 leftPos = Vec3(0, 0, m_triPodDimms->width/2);
Mat temp;
m_actorTop->getGlobalPose().getColumnMajor44((NxF32*)&temp);
temp._41 = 0;
temp._42 = 0;
temp._43 = 0;
D3DXVec3TransformCoord(&leftPos, &leftPos, &temp);
//NxRevoluteJointDesc revDescBottomLeft;
//revDescBottomLeft.actor[0] = m_actorBottom;
//revDescBottomLeft.actor[1] = m_attachedDevice->getActor();
//revDescBottomLeft.localNormal[0] = NxVec3(0, 1, 0);
//revDescBottomLeft.localNormal[1] = NxVec3(0, 1, 0);
//revDescBottomLeft.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescBottomLeft.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod + leftPos)); //Reference point that the axis passes through.
//m_jointBottomLeftPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescBottomLeft);
//NxRevoluteJointDesc revDescBottomRight;
//revDescBottomRight.actor[0] = m_actorBottom;
//revDescBottomRight.actor[1] = m_attachedDevice->getActor();
//revDescBottomRight.localNormal[0] = NxVec3(0, 1, 0);
//revDescBottomRight.localNormal[1] = NxVec3(0, 1, 0);
//revDescBottomRight.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescBottomRight.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod - leftPos)); //Reference point that the axis passes through.
//m_jointBottomRightPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescBottomRight);
//if(m_f
NxD6JointDesc d6Desc;
d6Desc.actor[0] = m_actorTop;
d6Desc.actor[1] = m_attachedDevice->getActor();
d6Desc.localNormal[0] = NxVec3(0, 1, 0);
d6Desc.localNormal[1] = NxVec3(0, 1, 0);
d6Desc.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posTopPod));
d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.projectionMode = NX_JPM_NONE;
d6Desc.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc.projectionDistance = 0.01f;
m_jointTopPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc);
NxD6JointDesc d6Desc1;
d6Desc1.actor[0] = m_actorBottom;
d6Desc1.actor[1] = m_attachedDevice->getActor();
d6Desc1.localNormal[0] = NxVec3(0, 1, 0);
d6Desc1.localNormal[1] = NxVec3(0, 1, 0);
d6Desc1.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc1.setGlobalAnchor(NxVec3((m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod) + leftPos));
d6Desc1.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc1.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc1.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc1.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.projectionMode = NX_JPM_NONE;
d6Desc1.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc1.projectionDistance = 0.01f;
m_jointBottomLeftPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc1);
NxD6JointDesc d6Desc2;
d6Desc2.actor[0] = m_actorBottom;
d6Desc2.actor[1] = m_attachedDevice->getActor();
d6Desc2.localNormal[0] = NxVec3(0, 1, 0);
d6Desc2.localNormal[1] = NxVec3(0, 1, 0);
d6Desc2.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc2.setGlobalAnchor(NxVec3((m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod) - leftPos));
d6Desc2.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc2.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc2.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc2.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.projectionMode = NX_JPM_NONE;
d6Desc2.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc2.projectionDistance = 0.01f;
m_jointBottomRightPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc2);
return;
}