void ConvexApproximationObject::AddPolygonFromObject(INode* const node, ObjectState* const os, NewtonMesh* const meshOut, const dMatrix& matrix)
{
	BOOL needDel = FALSE;
	PolyObject* const poly = GetPolyObject (os, needDel); 
	if (poly) {

		float polygon[32][12];
		memset (polygon, 0, sizeof (polygon));

		MNMesh& maxMesh = poly->GetMesh();
		int facesCount = maxMesh.FNum();
		int vertexCount = maxMesh.VNum();

		if (facesCount && vertexCount) {
			for (int i = 0; i < facesCount; i ++) {
				MNFace* const face = maxMesh.F(i);

				for (int j = 0; j < face->deg; j ++) {
					int index = face->vtx[j];
					Point3 p (maxMesh.P(index));
					dVector v (matrix.TransformVector(dVector (p.x, p.y, p.z, 0.0f)));
					polygon[j][0] = v.m_x;
					polygon[j][1] = v.m_y;
					polygon[j][2] = v.m_z;
				}
				NewtonMeshAddFace(meshOut, face->deg, &polygon[0][0], 12 * sizeof (float), 0);
			}
		}
	}
	if (needDel) {
		delete poly;
	}
}
示例#2
0
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
	for (int i = 0; i < 3; i ++) {
		dVector support(0.0f);
		dVector dir (0.0f);
		dir[i] = 1.0f;

		dVector localDir (matrix.UnrotateVector (dir));
		NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
		support = matrix.TransformVector (support);
		maxP[i] = support[i];  

		localDir = localDir.Scale (-1.0f);
		NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
		support = matrix.TransformVector (support);
		minP[i] = support[i];  
	}
}
示例#3
0
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
	dFloat skinThickness = NewtonCollisionGetSkinThickness (collision) * 0.125f;
	for (int i = 0; i < 3; i ++) {
		dVector support;
		dVector dir (0.0f, 0.0f, 0.0f, 0.0f);
		dir[i] = 1.0f;

		dVector localDir (matrix.UnrotateVector (dir));
		NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
		support = matrix.TransformVector (support);
		maxP[i] = support[i] - skinThickness;  

		localDir = localDir.Scale (-1.0f);
		NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
		support = matrix.TransformVector (support);
		minP[i] = support[i] + skinThickness;  
	}
}
示例#4
0
void MSNewton::Servo::adjust_pin_matrix_proc(JointData* joint_data, dMatrix& pin_matrix) {
	dMatrix matrix;
	dVector centre;
	NewtonBodyGetMatrix(joint_data->child, &matrix[0][0]);
	NewtonBodyGetCentreOfMass(joint_data->child, &centre[0]);
	centre = matrix.TransformVector(centre);
	centre = pin_matrix.UntransformVector(centre);
	dVector point(0.0f, 0.0f, centre.m_z);
	pin_matrix.m_posit = pin_matrix.TransformVector(point);
}
		void ApplySuspesionForce (
			dFloat timestep,
			const NewtonBody* thread, const dVector& threadPointLocal, const dMatrix& threadMatrix, const dVector& threadCOM, const dVector& threadVeloc, const dVector& threadOmega,
			const NewtonBody* parent, const dVector& parentPointLocal, const dMatrix& parentMatrix, const dVector& parentCOM, const dVector& parentVeloc, const dVector& parentOmega)
		{
			dFloat dist;
			dFloat speed;
			dFloat forceMag;

			// calculate separation and speed of hard points
			dVector threadPoint (threadMatrix.TransformVector(threadPointLocal));
			dVector parentPoint (parentMatrix.TransformVector(parentPointLocal));
			dist = (parentPoint - threadPoint) % parentMatrix.m_up;
			speed = ((parentVeloc + parentOmega * (parentPoint - parentCOM) -
					  threadVeloc - threadOmega * (threadPoint - threadCOM)) % parentMatrix.m_up);

			if (dist > MAX_COMPRESION_DIST) {
				NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
				NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
			} else if (dist < MIN_EXPANSION_DIST) {
				// submit a contact constraint to prevent the body 
				NewtonUserJointAddLinearRow (m_joint, &threadPoint[0], &threadPoint[0], &parentMatrix.m_up[0]);
				NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
			} 

			// apply the spring force
			forceMag = NewtonCalculateSpringDamperAcceleration (timestep, SPRING_CONST, dist, DAMPER_CONST, speed) * m_massScale;


			dVector forceParent (parentMatrix.m_up.Scale (forceMag));
			dVector torqueParent ((parentPoint - parentCOM) * forceParent);
			NewtonBodyAddForce(m_body1, &forceParent[0]);
			NewtonBodyAddTorque(m_body1, &torqueParent[0]);
			
		
			dVector forceThread (forceParent.Scale (-1.0f));
			dVector torqueThread ((threadPoint - threadCOM) * forceThread);
			NewtonBodyAddForce(m_body0, &forceThread[0]);
			NewtonBodyAddTorque(m_body0, &torqueThread[0]);
		}
示例#6
0
void CustomDGRayCastCar::CalculateTireCollision (Tire& tire, const dMatrix& suspensionMatrix, int threadIndex) const
{
	int floorcontact = 0;
	tire.m_HitBody = NULL;

	tire.m_posit = tire.m_suspensionLenght;
	if ( tire.m_tireUseConvexCastMode ) {
		dFloat hitParam;
		NewtonWorldConvexCastReturnInfo info;

//		tire.m_rayDestination = tire.m_suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght ));   
		dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght)));   
		if ( NewtonWorldConvexCast ( m_world, &suspensionMatrix[0][0], &rayDestination[0], tire.m_shape, &hitParam, (void*)m_body0, ConvexCastPrefilter, &info, 1, threadIndex ) ){
			tire.m_posit = hitParam * tire.m_suspensionLenght;
			tire.m_contactPoint = info.m_point;
			tire.m_contactNormal = info.m_normal;
			tire.m_HitBody = (NewtonBody*)info.m_hitBody; 
			floorcontact = 1;
		}
	} else {
		struct RayCastInfo
		{
			RayCastInfo (const NewtonBody* body)
			{
				m_param = 1.0f;
				m_me = body;
				m_hitBody = NULL;
				m_contactID = 0;
				m_normal = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			}
			static dFloat RayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
			{
				RayCastInfo& caster = *( (RayCastInfo*) userData ); 
				// if this body is not the vehicle, see if a close hit
				if ( body != caster.m_me ) {
					if ( intersetParam < caster.m_param) {
						// this is a close hit, record the information. 
						caster.m_param = intersetParam;
						caster.m_hitBody = body;
						caster.m_contactID = collisionID;
						caster.m_normal = dVector (normal[0], normal[1], normal[2], 1.0f);
					} 
				}
				return intersetParam;
			}
			dFloat m_param;
			dVector m_normal;
			const NewtonBody* m_me;
			const NewtonBody* m_hitBody;
			int m_contactID;
		};
		RayCastInfo info (m_body0);

_ASSERTE (0);
		// extend the ray by the radius of the tire
		dFloat dist ( tire.m_suspensionLenght + tire.m_radius );
		dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -dist )));	
		// cast a ray to the world ConvexCastPrefilter
		NewtonWorldRayCast( m_world, &suspensionMatrix.m_posit[0], &rayDestination[0], RayCastInfo::RayCast, &info, &ConvexCastPrefilter );
		// if the ray hit something, it means the tire has some traction
		if ( info.m_hitBody ) {
			dFloat intesectionDist;
			tire.m_HitBody = (NewtonBody*)info.m_hitBody; 
			tire.m_contactPoint = suspensionMatrix.m_posit + (rayDestination - suspensionMatrix.m_posit ).Scale ( info.m_param ); 
			tire.m_contactNormal = info.m_normal;  

			// TO DO: get the material properties for tire frictions on different roads 
			intesectionDist = ( dist * info.m_param - tire.m_radius );
			if ( intesectionDist < 0.0f ) {
				intesectionDist = 0.0f;
			} else if ( intesectionDist > tire.m_suspensionLenght ) {
				intesectionDist = tire.m_suspensionLenght;
			}
			tire.m_posit = intesectionDist;
		}
	}
}