Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}