Exemplo n.º 1
0
static void UserContactFriction (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	// call  the basic call back
	GenericContactProcess (contactJoint, timestep, threadIndex);

	const NewtonBody* const body0 = NewtonJointGetBody0(contactJoint);
	const NewtonBody* const body1 = NewtonJointGetBody1(contactJoint);
	const NewtonBody* body = body0;
	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
	if (mass == 0.0f) {
		body = body1;
	}

	//now core 300 can have per collision user data 		
	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	void* userData = NewtonCollisionGetUserData (collision);
	dFloat frictionValue = *((dFloat*)&userData);
	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
		NewtonMaterial* const material = NewtonContactGetMaterial (contact);
		NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 0);
		NewtonMaterialSetContactFrictionCoef (material, frictionValue + 0.1f, frictionValue, 1);
	}
}
Exemplo n.º 2
0
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	const dFloat stiffness = 0.33f;
	const dFloat damping = -0.05f;

	NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);

	// calculate the desired impulse
	dVector posit(targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
	dVector impulse(posit.Scale(stiffness * mass));

	// apply linear impulse
	NewtonBodyApplyImpulseArray(body, 1, sizeof (dVector), &impulse[0], &pointOnBodyInGlobalSpace[0], timestep);

	// apply linear and angular damping
	dMatrix inertia;
	dVector linearMomentum(0.0f);
	dVector angularMomentum(0.0f);

	NewtonBodyGetOmega(body, &angularMomentum[0]);
	NewtonBodyGetVelocity(body, &linearMomentum[0]);


	NewtonBodyGetInertiaMatrix(body, &inertia[0][0]);

	angularMomentum = inertia.RotateVector(angularMomentum);
	angularMomentum = angularMomentum.Scale(damping);
	linearMomentum = linearMomentum.Scale(mass * damping);

	NewtonBodyApplyImpulsePair(body, &linearMomentum[0], &angularMomentum[0], timestep);
}
Exemplo n.º 3
0
	static dFloat RayCastFilter (const NewtonBody* const body, const NewtonCollision* const collisionHit, const dFloat* const contact, const dFloat* const normal, dLong collisionID, void* const userData, dFloat intersetParam)
	{
		dFloat mass;
		dFloat Ixx;
		dFloat Iyy;
		dFloat Izz;

		// check if we are hitting a sub shape
		const NewtonCollision* const parent = NewtonCollisionGetParentInstance(collisionHit);
		if (parent) {
			// you can use this to filter sub collision shapes.  
			dAssert (NewtonCollisionGetSubCollisionHandle (collisionHit));
		}

		dMousePickClass* const data = (dMousePickClass*) userData;
		NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
		if ((mass > 0.0f) || (NewtonBodyGetType(body) == NEWTON_KINEMATIC_BODY)) {
			data->m_body = body;
		}

		if (intersetParam < data->m_param) {
			data->m_param = intersetParam;
			data->m_normal = dVector (normal[0], normal[1], normal[2]);
		}

		return intersetParam;
	}
Exemplo n.º 4
0
static void PhysicsNewton_CollisionPuckSurfaceCB(const NewtonJoint *pContactJoint,dFloat fTimeStep,int ThreadIndex)
{
	dVector Position(0.0f);
	
	// Get pointer to body
	NewtonBody* body = NewtonJointGetBody0(pContactJoint);				
	dFloat mass, Ixx, Iyy, Izz;
	NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
	if (mass == 0.0f)
	{
		body = NewtonJointGetBody1(pContactJoint);
	}	

	dVector tableDir(0.0f, 0.0f, 1.0f, 0.0f);
	// Test to see if it is the friction calculation that is causing the side force
	// With this the Puck must go straight because it will be frictionless, the net force should not change direction
	for (void* contact = NewtonContactJointGetFirstContact (pContactJoint); contact; contact = NewtonContactJointGetNextContact (pContactJoint, contact))
	{
		NewtonMaterial* const material = NewtonContactGetMaterial (contact);

		NewtonMaterialContactRotateTangentDirections(material, &tableDir[0]);

		// this the wrong way to make a friction less contact  
//		NewtonMaterialSetContactFrictionCoef (material, 0.0f, 0.0f, 0);
//		NewtonMaterialSetContactFrictionCoef (material, 0.0f, 0.0f, 1);

		//This is the correct way to make a friction less contact		
//		NewtonMaterialSetContactFrictionState (material, 0, 0);
//		NewtonMaterialSetContactFrictionState (material, 0, 1);
	}
}
Exemplo n.º 5
0
static void UserContactRestitution (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBody* body;
	NewtonBody* body0;
	NewtonBody* body1;

	// call  the basic call back
	GenericContactProcess (contactJoint, timestep, threadIndex);

	body0 = NewtonJointGetBody0(contactJoint);
	body1 = NewtonJointGetBody1(contactJoint);

	body = body0;
	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
	if (mass == 0.0f) {
		body = body1;
	}

	NewtonCollision* const collision = NewtonBodyGetCollision(body);
	void* userData = NewtonCollisionGetUserData (collision);
	dFloat restitution = *((dFloat*)&userData);
	for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
		NewtonMaterial* const material = NewtonContactGetMaterial (contact);
		NewtonMaterialSetContactElasticity (material, restitution);
	}
}
Exemplo n.º 6
0
	void ScaleIntertia(NewtonBody* const body, dFloat factor) const
	{
		dFloat Ixx;
		dFloat Iyy;
		dFloat Izz;
		dFloat mass;
		NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
		NewtonBodySetMassMatrix(body, mass, Ixx * factor, Iyy * factor, Izz * factor);
	}
void CustomKinematicController::SetMaxLinearFriction(dFloat accel)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
	m_maxLinearFriction = dAbs (accel) * mass;
}
static void ApplyGravity(const NewtonBody* const body, dFloat timestep, int threadIndex)
{
	// apply gravity force to the body
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;

	NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
	dVector gravityForce(0.0f, -9.8f * mass, 0.0f, 0.0f);
	NewtonBodySetForce(body, &gravityForce[0]);
}
Exemplo n.º 9
0
// add force and torque to rigid body
void  PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;

	dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
	NewtonBodySetForce (body, &force.m_x);
}
Exemplo n.º 10
0
void dNewtonDynamicBody::InitForceAccumulators()
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;

	NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
	const dNewtonWorld* const world = (dNewtonWorld*)NewtonWorldGetUserData(NewtonBodyGetWorld(m_body));

	m_externalForce = world->GetGravity().Scale(mass);
	m_externalTorque = dVector(0.0f);
}
Exemplo n.º 11
0
void Friction (DemoEntityManager* const scene)
{
	// load the skybox
	scene->CreateSkyBox();


	// load the scene from a ngd file format
	char fileName[2048];
	dGetWorkingFileName ("frictionDemo.ngd", fileName);
	scene->LoadScene (fileName);


	// set a default material call back
	NewtonWorld* const world = scene->GetNewton();
	int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
	NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, UserOnAABBOverlap, UserContactFriction); 
	//NewtonMaterialSetDefaultCollidable(world, defaultMaterialID, defaultMaterialID, 0);

	// customize the scene after loading
	// set a user friction variable in the body for variable friction demos
	// later this will be done using LUA script
	int index = 0;
	for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) {
		dFloat Ixx;
		dFloat Iyy;
		dFloat Izz;
		dFloat mass;
		NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
		if (mass > 0.0f) {
			// use the new instance feature to ass per shape information
			NewtonCollision* const collision = NewtonBodyGetCollision(body);

			// use the collision user data to save the coefficient of friction 
			dFloat coefficientOfFriction = dFloat (index) * 0.03f; 
			NewtonCollisionSetUserData (collision, *((void**)&coefficientOfFriction));

//			DemoEntity* const entity = (DemoEntity*) NewtonBodyGetUserData (body);
//			dVariable* const friction = entity->CreateVariable (FRICTION_VAR_NAME);
//			dAssert (friction);
//			friction->SetValue(dFloat (index) * 0.03f);
			index ++;
		}
	}

	// place camera into position
	dQuaternion rot;
	dVector origin (-70.0f, 10.0f, 0.0f, 0.0f);
	scene->SetCameraMatrix(rot, origin);

//	ExportScene (scene->GetNewton(), "../../../media/test1.ngd");
}
Exemplo n.º 12
0
void NewtonRigidBodySetForceCB(const NewtonBody* const body, dFloat timestep, int threadIndex)
{	
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
	
	dFloat force[3];
	force[0] = 0.0f;
	force[1] = mass * (DEMO_GRAVITY * PHYSICS_WORLD_SCALE);
	force[2] = 0.0f;
	NewtonBodySetForce(body, force);
}
Exemplo n.º 13
0
static void RenderBodyContactsForces (NewtonBody* const body, dFloat scale)
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);	

	//draw normal forces in term of acceleration.
	//this  mean that two bodies with same shape but different mass will display the same force
	if (mass > 0.0f) {
		scale = scale/mass;
		for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
			if (NewtonJointIsActive (joint)) {
				for (void* contact = NewtonContactJointGetFirstContact (joint); contact; contact = NewtonContactJointGetNextContact (joint, contact)) {
					dVector point(0.0f);
					dVector normal(0.0f);	
					dVector tangnetDir0(0.0f);
					dVector tangnetDir1(0.0f);
					dVector contactForce(0.0f);	
					NewtonMaterial* const material = NewtonContactGetMaterial (contact);

					NewtonMaterialGetContactForce(material, body, &contactForce.m_x);
					NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
					dVector normalforce (normal.Scale (contactForce.DotProduct3(normal)));
					dVector p0 (point);
					dVector p1 (point + normalforce.Scale (scale));
					glVertex3f (p0.m_x, p0.m_y, p0.m_z);
					glVertex3f (p1.m_x, p1.m_y, p1.m_z);

					// these are the components of the tangents forces at the contact point, the can be display at the contact position point.
					NewtonMaterialGetContactTangentDirections(material, body, &tangnetDir0[0], &tangnetDir1[0]);
					dVector tangentForce1 (tangnetDir0.Scale ((contactForce.DotProduct3(tangnetDir0)) * scale));
					dVector tangentForce2 (tangnetDir1.Scale ((contactForce.DotProduct3(tangnetDir1)) * scale));

					p1 = point + tangentForce1.Scale (scale);
					glVertex3f(p0.m_x, p0.m_y, p0.m_z);
					glVertex3f(p1.m_x, p1.m_y, p1.m_z);

					p1 = point + tangentForce2.Scale (scale);
					glVertex3f(p0.m_x, p0.m_y, p0.m_z);
					glVertex3f(p1.m_x, p1.m_y, p1.m_z);
				}
			}
		}
	}
}
Exemplo n.º 14
0
void dNewtonBody::SetCenterOfMass(float com_x, float com_y, float com_z)
{
	dVector com;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);
	NewtonCollision* const collision = NewtonBodyGetCollision(m_body);
	NewtonBodySetMassProperties(m_body, mass, NewtonBodyGetCollision(m_body));
	NewtonBodyGetCentreOfMass (m_body, &com[0]);
	com.m_x += com_x;
	com.m_y += com_y;
	com.m_z += com_z;
	NewtonBodySetCentreOfMass(m_body, &com[0]);
}
void CustomKinematicController::SetMaxAngularFriction(dFloat alpha)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
//	if (Iyy > Ixx) {
//		Ixx = Iyy;
//	}
//	if (Izz > Ixx) {
//		Ixx = Izz;
//	}
//	m_maxAngularFriction = dAbs (alpha) * Ixx;
	m_maxAngularFriction = dAbs (alpha) * mass;
}
Exemplo n.º 16
0
void dNewtonBody::CalculateBuoyancyForces(const void* plane, void* force, void* torque, float bodyDensity)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz);

	if (mass > 0.0f) {
		dMatrix matrix;
		dVector cog(0.0f);
		dVector accelPerUnitMass(0.0f);
		dVector torquePerUnitMass(0.0f);
		const dVector gravity(0.0f, -9.8f, 0.0f, 0.0f);

		NewtonBodyGetMatrix(m_body, &matrix[0][0]);
		NewtonBodyGetCentreOfMass(m_body, &cog[0]);
		cog = matrix.TransformVector(cog);
		NewtonCollision* const collision = NewtonBodyGetCollision(m_body);

		dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision);
		dFloat fluidDensity = 1.0f / (bodyDensity * shapeVolume);
		dFloat viscosity = 0.995f;

		NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity[0], (float*)plane, fluidDensity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);

		dVector finalForce(accelPerUnitMass.Scale(mass));
		dVector finalTorque(torquePerUnitMass.Scale(mass));

		dVector omega(0.0f);
		NewtonBodyGetOmega(m_body, &omega[0]);
		omega = omega.Scale(viscosity);
		NewtonBodySetOmega(m_body, &omega[0]);

		((float*)force)[0] = finalForce.m_x ;
		((float*)force)[1] = finalForce.m_y ;
		((float*)force)[2] = finalForce.m_z ;
		((float*)torque)[0] = finalTorque.m_x;
		((float*)torque)[1] = finalTorque.m_y;
		((float*)torque)[2] = finalTorque.m_z;
	}
}
Exemplo n.º 17
0
dFloat ForceBodyAccelerationMichio (NewtonBody* const body)
{
	dVector reactionforce (0.0f);
	// calcualte accelration generate by all contacts
	for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
		if (NewtonJointIsActive(joint)) {
			for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
				dVector contactForce(0.0f);
				NewtonMaterial* const material = NewtonContactGetMaterial(contact);
				NewtonMaterialGetContactForce(material, body, &contactForce[0]);
				reactionforce += contactForce;
			}
		}
	}

	dMatrix matrix;
	dVector accel;
	dVector veloc;

	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
	NewtonBodyGetAcceleration(body, &accel[0]);
	accel -= reactionforce.Scale (1.0f/mass);


	//calculate centripetal acceleration here.
	NewtonBodyGetMatrix(body, &matrix[0][0]);
	dVector radius(matrix.m_posit.Scale(-1.0f));
	radius.m_w = 0.0f;
	dFloat radiusMag = dSqrt(radius.DotProduct3(radius));
	dVector radiusDir (radius.Normalize());
	
	NewtonBodyGetVelocity(body, &veloc[0]);
	veloc += radiusDir.Scale(veloc.DotProduct3(radiusDir));

	dVector centripetalAccel(veloc.DotProduct3(veloc) / radiusMag);
	accel += centripetalAccel;
	return dSqrt (accel.DotProduct3(accel));
}
Exemplo n.º 18
0
// add force and torque to rigid body
void  PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
	dVector dir(0.0f, 1.0f, 0.0f);
//	dVector dir(1.0f, 0.0f, 0.0f);
//mass = 0.0f;
	dVector force (dir.Scale (mass * DEMO_GRAVITY));
	NewtonBodySetForce (body, &force.m_x);

#ifdef DEMO_CHECK_ASYN_UPDATE
	dAssert(g_checkAsyncUpdate);
#endif

	// test going to sleep bug
//	NewtonBodySetSleepState(body, 0);
}
dVehicleSingleBody::dVehicleSingleBody(dVehicleChassis* const chassis)
	:dVehicleInterface(chassis)
	,m_gravity(0.0f)
	,m_groundNode(NULL)
	,m_newtonBody(chassis->GetBody())
{
	dVector tmp;
	dComplementaritySolver::dBodyState* const chassisBody = GetBody();
	m_groundNode.SetWorld(m_world);
	m_groundNode.SetLoopNode(true);
	
	// set the inertia matrix;
	NewtonBodyGetMass(m_newtonBody, &tmp.m_w, &tmp.m_x, &tmp.m_y, &tmp.m_z);
	chassisBody->SetMass(tmp.m_w);
	chassisBody->SetInertia(tmp.m_x, tmp.m_y, tmp.m_z);

	dMatrix matrix (dGetIdentityMatrix());
	NewtonBodyGetCentreOfMass(m_newtonBody, &matrix.m_posit[0]);
	matrix.m_posit.m_w = 1.0f;
	chassisBody->SetLocalMatrix(matrix);
}
Exemplo n.º 20
0
	void SetModelMass( dFloat mass, dAnimationJointRoot* const rootNode) const
	{
		dFloat volume = 0.0f;
		for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
			volume += NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(joint->GetBody()));
		}
		dFloat density = mass / volume;

		for (dAnimationJoint* joint = GetFirstJoint(rootNode); joint; joint = GetNextJoint(joint)) {
			dFloat Ixx;
			dFloat Iyy;
			dFloat Izz;
			NewtonBody* const body = joint->GetBody();
			NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
			dFloat scale = density * NewtonConvexCollisionCalculateVolume(NewtonBodyGetCollision(body));
			mass *= scale;
			Ixx *= scale;
			Iyy *= scale;
			Izz *= scale;
			NewtonBodySetMassMatrix(body, mass, Ixx, Iyy, Izz);
		}
	}
Exemplo n.º 21
0
// add force and torque to rigid body
void  PhysicsApplyGravityForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);
//mass*= 0.0f;

	dVector force (dVector (0.0f, 1.0f, 0.0f).Scale (mass * DEMO_GRAVITY));
	NewtonBodySetForce (body, &force.m_x);
/*
// check that angular momentum is conserved
dMatrix I;
dVector omega(0.0f);
NewtonBodyGetInertiaMatrix(body, &I[0][0]);
NewtonBodyGetOmega(body, &omega[0]);
dVector L (I.RotateVector(omega));
dTrace (("(%f %f %f) (%f %f %f)\n", omega[0], omega[1], omega[2], L[0], L[1], L[2]));
*/
}
Exemplo n.º 22
0
		void OnInside(NewtonBody* const visitor)
		{
			dFloat Ixx;
			dFloat Iyy;
			dFloat Izz;
			dFloat mass;
			
			NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz);
			if (mass > 0.0f) {
				dMatrix matrix;
				dVector cog(0.0f);
				dVector accelPerUnitMass(0.0f);
				dVector torquePerUnitMass(0.0f);
				const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f);

				NewtonBodyGetMatrix (visitor, &matrix[0][0]);
				NewtonBodyGetCentreOfMass(visitor, &cog[0]);
				cog = matrix.TransformVector (cog);
				NewtonCollision* const collision = NewtonBodyGetCollision(visitor);

				
				dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
				dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume);
				dFloat viscosity = 0.995f;

				NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);

				dVector force (accelPerUnitMass.Scale (mass));
				dVector torque (torquePerUnitMass.Scale (mass));

				dVector omega(0.0f); 
				NewtonBodyGetOmega(visitor, &omega[0]);
				omega = omega.Scale (viscosity);
				NewtonBodySetOmega(visitor, &omega[0]);

				NewtonBodyAddForce (visitor, &force[0]);
				NewtonBodyAddTorque (visitor, &torque[0]);
			}
		}
Exemplo n.º 23
0
	static void ClampAngularVelocity(const NewtonBody* body, dFloat timestep, int threadIndex)
	{
		dVector omega;
		NewtonBodyGetOmega(body, &omega[0]);
		omega.m_w = 0.0f;
		dFloat mag2 = omega.DotProduct3(omega);
		if (mag2 > (100.0f * 100.0f)) {
			omega = omega.Normalize().Scale(100.0f);
			NewtonBodySetOmega(body, &omega[0]);
		}

		//PhysicsApplyGravityForce(body, timestep, threadIndex);
		dFloat Ixx;
		dFloat Iyy;
		dFloat Izz;
		dFloat mass;

		dFloat gravity = -0.0f;
		NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
		dVector dir(0.0f, gravity, 0.0f);
		dVector force(dir.Scale(mass));
		NewtonBodySetForce(body, &force.m_x);
	}
dCustomTireSpringDG::dCustomTireSpringDG(const dMatrix& pinAndPivotFrameChild, const dMatrix& pinAndPivotFrameParent, NewtonBody* const child, NewtonBody* const parent)
	:dCustomJoint(6, child, parent),
	mUseBreak(false),
	mUseSteer(false),
	mUseTorque(false),
	mUseHardBreak(false),
	mIxx(0.0f),
	mIyy(0.0f),
	mIzz(0.0f),
	mRealOmega(0.0f),
	mTireOmegaCorrection(0.975f),
	mFpsRequest(120.0f),
	mTireTorque(0.0f),
	mBrakeTorque(0.0f),
	mSteerAngle(0.0f),
	mAttachmentLength(10.0f),
	mDistance(0.0f),
	mMinSuspenssion(-0.25f),
	mMaxSuspenssion(0.0f),
	mSpringK(150.0f),
	mSpringD(5.0f),
	mSpringMassEffective(0.5f),
	mAccel(0.0f),
	mAttachMass(0.0f),
	mCenterInTire(dVector(0.0f, 0.0f, 0.0f)),
	mCenterInChassis(dVector(0.0f, 0.0f, 0.0f)),
	mChassisPivotMatrix(dGetIdentityMatrix()),
	mTirePivotMatrix(dGetIdentityMatrix()),
	mRefFrameLocalMatrix(dGetIdentityMatrix())
{
	dMatrix	dummy;
	CalculateLocalMatrix(pinAndPivotFrameChild, m_localMatrix0, dummy);
	CalculateLocalMatrix(pinAndPivotFrameParent, dummy, m_localMatrix1);
    //
    mRefFrameLocalMatrix = m_localMatrix0;
	NewtonBodyGetMass(parent, &mAttachMass, &mIxx, &mIyy, &mIzz);
}
	void SimulationPostListener(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep)
	{
		// see if the net force on the body comes fr a high impact collision
		dFloat breakImpact = 0.0f;
		for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(m_myBody); joint; joint = NewtonBodyGetNextContactJoint(m_myBody, joint)) {
			for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
				dVector contactForce;
				NewtonMaterial* const material = NewtonContactGetMaterial(contact);
				dFloat impulseImpact = NewtonMaterialGetContactMaxNormalImpact(material);
				if (impulseImpact > breakImpact) {
					breakImpact = impulseImpact;
				}
			}
		}


		// if the force is bigger than N time Gravities, It is considered a collision force
		breakImpact *= m_myMassInverse;
//		breakImpact = 1000.0f;
		if (breakImpact > BREAK_IMPACT_IN_METERS_PER_SECONDS) {
			NewtonWorld* const world = NewtonBodyGetWorld(m_myBody);

			dMatrix bodyMatrix;
			dVector com(0.0f);
			dVector veloc(0.0f);
			dVector omega(0.0f);
			dFloat Ixx;
			dFloat Iyy;
			dFloat Izz;
			dFloat mass;

			NewtonBodyGetVelocity(m_myBody, &veloc[0]);
			NewtonBodyGetOmega(m_myBody, &omega[0]);
			NewtonBodyGetCentreOfMass(m_myBody, &com[0]);
			NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]);
			NewtonBodyGetMass(m_myBody, &mass, &Ixx, &Iyy, &Izz);

			com = bodyMatrix.TransformVector(com);
			dMatrix matrix(GetCurrentMatrix());
			dQuaternion rotation(matrix);

			// we need to lock the world before creation a bunch of bodies
			scene->Lock(m_lock);

			for (FractureEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) {
				FractureAtom& atom = node->GetInfo();

				DemoEntity* const entity = new DemoEntity(dMatrix(rotation, matrix.m_posit), NULL);
				entity->SetMesh(atom.m_mesh, dGetIdentityMatrix());
				scene->Append(entity);

				int materialId = 0;

				dFloat debriMass = mass * atom.m_massFraction;

				//create the rigid body
				NewtonBody* const rigidBody = NewtonCreateDynamicBody(world, atom.m_collision, &matrix[0][0]);

				// calculate debris initial velocity
				dVector center(matrix.TransformVector(atom.m_centerOfMass));
				dVector v(veloc + omega.CrossProduct(center - com));

				// set initial velocity
				NewtonBodySetVelocity(rigidBody, &v[0]);
				NewtonBodySetOmega(rigidBody, &omega[0]);

				// set the debris mass properties, mass, center of mass, and inertia 
				NewtonBodySetMassProperties(rigidBody, debriMass, atom.m_collision);

				// save the pointer to the graphic object with the body.
				NewtonBodySetUserData(rigidBody, entity);

				// assign the wood id
				NewtonBodySetMaterialGroupID(rigidBody, materialId);

				//  set continuous collision mode
				//	NewtonBodySetContinuousCollisionMode (rigidBody, continueCollisionMode);

				// set a destructor for this rigid body
				NewtonBodySetDestructorCallback(rigidBody, PhysicsBodyDestructor);

				// set the transform call back function
				NewtonBodySetTransformCallback(rigidBody, DemoEntity::TransformCallback);

				// set the force and torque call back function
				NewtonBodySetForceAndTorqueCallback(rigidBody, PhysicsApplyGravityForce);
			}

			NewtonDestroyBody(m_myBody);
			scene->RemoveEntity(mynode);

			// unlock the work after done with the effect 
			scene->Unlock(m_lock);
		}
	}
Exemplo n.º 26
0
void DemoCameraListener::UpdatePickBody(DemoEntityManager* const scene, dFloat timestep) 
{
	// handle pick body from the screen
	bool mousePickState = scene->GetMouseKeyState(0);
	if (!m_targetPicked) {
		if (!m_prevMouseState && mousePickState) {
			dFloat param;
			dVector posit;
			dVector normal;

			dFloat x = dFloat (m_mousePosX);
			dFloat y = dFloat (m_mousePosY);
			dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
			dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
			NewtonBody* const body = MousePickBody (scene->GetNewton(), p0, p1, param, posit, normal);
			if (body) {
				dMatrix matrix;
				m_targetPicked = body;
				NewtonBodyGetMatrix(m_targetPicked, &matrix[0][0]);

				m_pickedBodyParam = param;
				#ifdef USE_PICK_BODY_BY_FORCE
					// save point local to the body matrix
					m_pickedBodyLocalAtachmentPoint = matrix.UntransformVector (posit);

					// convert normal to local space
					m_pickedBodyLocalAtachmentNormal = matrix.UnrotateVector(normal);
				#else
					if(m_pickJoint) {
						delete m_pickJoint;
						m_pickJoint = NULL;
					}
					
					dFloat Ixx;
					dFloat Iyy;
					dFloat Izz;
					dFloat mass;
					NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);

					const dFloat angularFritionAccel = 100.0f;
					const dFloat linearFrictionAccel = 100.0f * dAbs (dMax (DEMO_GRAVITY, 10.0f));
					const dFloat inertia = dMax (Izz, dMax (Ixx, Iyy));

					m_pickJoint = new dCustomKinematicController (body, posit);
					m_pickJoint->SetPickMode (0);
					m_pickJoint->SetMaxLinearFriction(mass * linearFrictionAccel);
					m_pickJoint->SetMaxAngularFriction(inertia * angularFritionAccel);
				#endif
			}
		}

	} else {
		if (scene->GetMouseKeyState(0)) {
			dFloat x = dFloat (m_mousePosX);
			dFloat y = dFloat (m_mousePosY);
			dVector p0 (m_camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
			dVector p1 (m_camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
			m_pickedBodyTargetPosition = p0 + (p1 - p0).Scale (m_pickedBodyParam);

			#ifdef USE_PICK_BODY_BY_FORCE
			dMatrix matrix;
			NewtonBodyGetMatrix (m_targetPicked, &matrix[0][0]);
			dVector point (matrix.TransformVector(m_pickedBodyLocalAtachmentPoint));
			CalculatePickForceAndTorque (m_targetPicked, point, m_pickedBodyTargetPosition, timestep);
			#else 
				if (m_pickJoint) {
					m_pickJoint->SetTargetPosit (m_pickedBodyTargetPosition); 
				}
			#endif
		} else {
			if (m_targetPicked) {
				NewtonBodySetSleepState (m_targetPicked, 0);
			}
			if (m_pickJoint) {
				delete m_pickJoint;
			}
			m_pickJoint = NULL;
			m_targetPicked = NULL; 
			m_bodyDestructor = NULL;
		}
	}

	m_prevMouseState = mousePickState;
}
Exemplo n.º 27
0
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
	dMatrix matrix; 
	dVector com(0.0f); 
	dVector omega0(0.0f);
	dVector veloc0(0.0f);
	dVector omega1(0.0f);
	dVector veloc1(0.0f);
	dVector pointVeloc(0.0f);

	const dFloat stiffness = 0.3f;
	const dFloat angularDamp = 0.95f;

	dFloat invTimeStep = 1.0f / timestep;
	NewtonWorld* const world = NewtonBodyGetWorld (body);
	NewtonWorldCriticalSectionLock (world, 0);

	// calculate the desired impulse
	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodyGetOmega (body, &omega0[0]);
	NewtonBodyGetVelocity (body, &veloc0[0]);

	NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]);

	dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace);
	deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc;
	for (int i = 0; i < 3; i ++) {
		dVector veloc (0.0f);
		veloc[i] = deltaVeloc[i];
		NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]);
	}

	// damp angular velocity
	NewtonBodyGetOmega (body, &omega1[0]);
	NewtonBodyGetVelocity (body, &veloc1[0]);
	omega1 = omega1.Scale (angularDamp);

	// restore body velocity and angular velocity
	NewtonBodySetOmega (body, &omega0[0]);
	NewtonBodySetVelocity(body, &veloc0[0]);

	// convert the delta velocity change to a external force and torque
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	NewtonBodyGetMass (body, &mass, &Ixx, &Iyy, &Izz);

	dVector angularMomentum (Ixx, Iyy, Izz);
	angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0)));

	dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep));
	dVector torque (angularMomentum.Scale(invTimeStep));

	NewtonBodyAddForce(body, &force[0]);
	NewtonBodyAddTorque(body, &torque[0]);

	// make sure the body is unfrozen, if it is picked
	NewtonBodySetSleepState (body, 0);

	NewtonWorldCriticalSectionUnlock (world);
}
Exemplo n.º 28
0
 void iPhysics::getMassMatrix(void* newtonBody, float32& mass, float32& Ixx, float32& Iyy, float32& Izz)
 {
     NewtonBodyGetMass(static_cast<const NewtonBody*>(newtonBody), &mass, &Ixx, &Iyy, &Izz);
 }