void CustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector veloc0;
	dVector veloc1;
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);
	
	// calculate the angular velocity for both bodies
	NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);

	// get angular velocity relative to the pin vector
	dFloat w0 = veloc0 % matrix0.m_front;
	dFloat w1 = veloc1 % matrix1.m_front;

	// establish the gear equation.
	dFloat relVeloc = w0 + m_gearRatio * w1;
	if (m_gearRatio > dFloat(1.0f)) {
		relVeloc = w0 / m_gearRatio + w1;
	}

	// calculate the relative angular acceleration by dividing by the time step
	// ideally relative acceleration should be zero, but is practice there will always 
	// be a small difference in velocity that need to be compensated. 
	// using the full acceleration will make the to over show a oscillate 
	// so use only fraction of the acceleration
	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;
	dFloat relAccel = - 0.3f * relVeloc * invTimestep;

	// set the linear part of Jacobian 0 to translational pin vector	
	jacobian0[0] = 	matrix0.m_front[0];
	jacobian0[1] = 	matrix0.m_front[1];
	jacobian0[2] = 	matrix0.m_front[2];

	// set the rotational part of Jacobian 0 to zero
	jacobian0[3] = 	0.0f;
	jacobian0[4] = 	0.0f;
	jacobian0[5] = 	0.0f;

	// set the linear part of Jacobian 1 to translational pin vector	
	jacobian1[0] = 	matrix1.m_front[0];
	jacobian1[1] = 	matrix1.m_front[1];
	jacobian1[2] = 	matrix1.m_front[2];

	// set the rotational part of Jacobian 1 to zero
	jacobian1[3] = 	0.0f;
	jacobian1[4] = 	0.0f;
	jacobian1[5] = 	0.0f;

	// add a angular constraint
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);

	// set the desired angular acceleration between the two bodies
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void MSP::PointToPoint::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) {
    MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint));
    PointToPointData* cj_data = reinterpret_cast<PointToPointData*>(joint_data->m_cj_data);

    dFloat inv_timestep = 1.0f / timestep;

    dMatrix matrix0, matrix1;
    MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1);

    dVector p0(matrix0.m_posit + matrix0.m_right.Scale(cj_data->m_start_distance));
    const dVector& p1 = matrix1.m_posit;

    dVector veloc0(0.0f);
    dVector veloc1(0.0f);
    NewtonBodyGetVelocity(joint_data->m_child, &veloc0[0]);
    if (joint_data->m_parent != nullptr)
        NewtonBodyGetVelocity(joint_data->m_parent, &veloc1[0]);
    dVector rel_veloc(veloc0 - veloc1);

    dFloat stiffness = 0.999f - (1.0f - joint_data->m_stiffness_ratio * cj_data->m_strength) * Joint::DEFAULT_STIFFNESS_RANGE;

    if (cj_data->m_mode == 0) {
        dVector dir(p0 - p1);
        cj_data->m_cur_distance = Util::get_vector_magnitude(dir);
        if (cj_data->m_cur_distance > M_EPSILON)
            Util::scale_vector(dir, 1.0f / cj_data->m_cur_distance);
        else {
            dir.m_x = 0.0f;
            dir.m_y = 0.0f;
            dir.m_z = 1.0f;
        }
        dFloat offset = cj_data->m_cur_distance - cj_data->m_start_distance * cj_data->m_controller;
        dFloat dir_veloc = rel_veloc.DotProduct3(dir);
        dFloat des_accel = cj_data->m_accel * -offset - dir_veloc * inv_timestep * cj_data->m_damp;
        NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &dir[0]);
        NewtonUserJointSetRowAcceleration(joint, des_accel);
        NewtonUserJointSetRowStiffness(joint, stiffness);
    }
    else {
        dVector offset(matrix1.UntransformVector(p0));
        cj_data->m_cur_distance = Util::get_vector_magnitude(offset);
        offset.m_z -= cj_data->m_start_distance * cj_data->m_controller;
        dVector loc_vel(matrix1.UnrotateVector(rel_veloc));
        dVector des_accel(offset.Scale(-cj_data->m_accel) - loc_vel.Scale(inv_timestep * cj_data->m_damp));

        NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_front[0]);
        NewtonUserJointSetRowAcceleration(joint, des_accel.m_x);
        NewtonUserJointSetRowStiffness(joint, stiffness);

        NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_up[0]);
        NewtonUserJointSetRowAcceleration(joint, des_accel.m_y);
        NewtonUserJointSetRowStiffness(joint, stiffness);

        NewtonUserJointAddLinearRow(joint, &p0[0], &p1[0], &matrix1.m_right[0]);
        NewtonUserJointSetRowAcceleration(joint, des_accel.m_z);
        NewtonUserJointSetRowStiffness(joint, stiffness);
    }
}
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);
}
void RigidBodyData::Save(ISave* const isave)
{
	ULONG nwrit;
	int revision = D_FILE_REVISION;
	dVector mass;
	dVector com;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	RigidBodyWorldDesc& me = *(RigidBodyWorldDesc*) RigidBodyWorldDesc::GetDescriptor();

	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	NewtonBodyGetVelocity(m_body, &veloc[0]);
	NewtonBodyGetOmega(m_body, &omega[0]);

	NewtonCollision* const collision = NewtonBodyGetCollision(m_body);

	isave->Write((const char*)&revision, sizeof (revision), &nwrit);
	isave->Write((const char*)&m_oldControlerID, sizeof (m_oldControlerID), &nwrit);
	isave->Write((const char*)&m_collisionShape, sizeof (m_collisionShape), &nwrit);
	isave->Write((const char*)&m_hideGizmos, sizeof (m_hideGizmos), &nwrit);
	isave->Write((const char*)&m_mass, sizeof (m_mass), &nwrit);
	isave->Write((const char*)&m_inertia, sizeof (m_inertia), &nwrit);
	isave->Write((const char*)&m_origin, sizeof (m_origin), &nwrit);

	isave->Write((const char*)&matrix, sizeof (matrix), &nwrit);
	isave->Write((const char*)&veloc, sizeof (veloc), &nwrit);
	isave->Write((const char*)&omega, sizeof (omega), &nwrit);
	NewtonCollisionSerialize (me.m_newton, collision, SaveCollision, isave);
}
Example #5
0
NzVector3f NzPhysObject::GetVelocity() const
{
	NzVector3f velocity;
	NewtonBodyGetVelocity(m_body, velocity);

	return velocity;
}
void dCustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector veloc0(0.0f);
	dVector veloc1(0.0f);
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// set the linear part of Jacobian 0 to translational pin vector	
	dVector dir0 (matrix0.m_front.Scale (1.0f/m_gearRatio));
	const dVector& dir1 = matrix1.m_front;
	jacobian0[0] = dir0.m_x;
	jacobian0[1] = dir0.m_y;	
	jacobian0[2] = dir0.m_z;
	jacobian0[3] = 0.0f;
	jacobian0[4] = 0.0f;
	jacobian0[5] = 0.0f;
	
	jacobian1[0] = dir1.m_x;
	jacobian1[1] = dir1.m_y;	
	jacobian1[2] = dir1.m_z;	
	jacobian1[3] = 0.0f;
	jacobian1[4] = 0.0f;
	jacobian1[5] = 0.0f;

	// calculate the angular velocity for both bodies
	NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);

	// get angular velocity relative to the pin vector
	dFloat w0 = veloc0.DotProduct3(dir0);
	dFloat w1 = veloc1.DotProduct3(dir1);
	dFloat relVeloc = w0 + w1;

	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
	dFloat relAccel = -0.5f * relVeloc * invTimestep;

	// add a angular constraint
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);

	// set the desired angular acceleration between the two bodies
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
dVector CustomPlayerController::CalculateDesiredVelocity (dFloat forwardSpeed, dFloat lateralSpeed, dFloat verticalSpeed, const dVector& gravity, dFloat timestep) const
{
	dMatrix matrix;
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	dVector updir (matrix.RotateVector(m_upVector));
	dVector frontDir (matrix.RotateVector(m_frontVector));
	dVector rightDir (frontDir * updir);

	dVector veloc (0.0f, 0.0f, 0.0f, 0.0f);
	if ((verticalSpeed <= 0.0f) && (m_groundPlane % m_groundPlane) > 0.0f) {
		// plane is supported by a ground plane, apply the player input velocity
		if ((m_groundPlane % updir) >= m_maxSlope) {
			// player is in a legal slope, he is in full control of his movement
			dVector bodyVeloc;
			NewtonBodyGetVelocity(m_body, &bodyVeloc[0]);
			veloc = updir.Scale(bodyVeloc % updir) + gravity.Scale (timestep) + frontDir.Scale (forwardSpeed) + rightDir.Scale (lateralSpeed) + updir.Scale(verticalSpeed);
			veloc += (m_groundVelocity - updir.Scale (updir % m_groundVelocity));

			dFloat speedLimitMag2 = forwardSpeed * forwardSpeed + lateralSpeed * lateralSpeed + verticalSpeed * verticalSpeed + m_groundVelocity % m_groundVelocity + 0.1f;
			dFloat speedMag2 = veloc % veloc;
			if (speedMag2 > speedLimitMag2) {
				veloc = veloc.Scale (dSqrt (speedLimitMag2 / speedMag2));
			}

			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		} else {
			// player is in an illegal ramp, he slides down hill an loses control of his movement 
			NewtonBodyGetVelocity(m_body, &veloc[0]);
			veloc += updir.Scale(verticalSpeed);
			veloc += gravity.Scale (timestep);
			dFloat normalVeloc = m_groundPlane % (veloc - m_groundVelocity);
			if (normalVeloc < 0.0f) {
				veloc -= m_groundPlane.Scale (normalVeloc);
			}
		}
	} else {
		// player is on free fall, only apply the gravity
		NewtonBodyGetVelocity(m_body, &veloc[0]);
		veloc += updir.Scale(verticalSpeed);
		veloc += gravity.Scale (timestep);
	}
	return veloc;
}
dFloat CustomMultiBodyVehicle::GetSpeed() const
{
    dVector veloc;
    dMatrix chassisMatrix;

    NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
    NewtonBodyGetVelocity(m_body0, &veloc[0]);
    return veloc % chassisMatrix.m_front;
}
	dVector BodyGetPointVelocity(const NewtonBody* const body, const dVector &point)
	{
		dVector v, w, c;
		NewtonBodyGetVelocity(body, &v[0]);
		NewtonBodyGetOmega(body, &w[0]);
		dMatrix matrix;
		NewtonBodyGetMatrix(body, &matrix[0][0]);
		c = matrix.m_posit; // TODO: Does not handle COM offset !!!
		return v + w * (point - c);
	}
Example #10
0
static void PhysicsApplyPickForce (const NewtonBody* body, dFloat timestep, int threadIndex)
{
	dFloat mass;
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dVector com;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	// apply the thew body forces
	if (chainForceCallback) {
		chainForceCallback (body, timestep, threadIndex);
	}

	// add the mouse pick penalty force and torque
	NewtonBodyGetVelocity(body, &veloc[0]);

	NewtonBodyGetOmega(body, &omega[0]);
	NewtonBodyGetVelocity(body, &veloc[0]);
	NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);

	dVector force (pickedForce.Scale (mass * MOUSE_PICK_STIFFNESS));
	dVector dampForce (veloc.Scale (MOUSE_PICK_DAMP * mass));
	force -= dampForce;

	NewtonBodyGetMatrix(body, &matrix[0][0]);
	NewtonBodyGetCentreOfMass (body, &com[0]);
	
	// calculate local point relative to center of mass
	dVector point (matrix.RotateVector (attachmentPoint - com));
	dVector torque (point * force);

	dVector torqueDamp (omega.Scale (mass * 0.1f));

	NewtonBodyAddForce (body, &force.m_x);
	NewtonBodyAddTorque (body, &torque.m_x);

	// make sure the body is unfrozen, if it is picked
	NewtonBodySetFreezeState (body, 0);
}
		void SubmitConstraints (dFloat timestep, int threadIndex)
		{
			// calculate suspension bumpers and forces
			dMatrix threadMatrix;
			dMatrix parentMatrix;

			dVector threadCOM;
			dVector parentCOM;
			dVector threadVeloc;
			dVector parentVeloc;
			dVector threadOmega;
			dVector parentOmega;


			// get the physics body state;
			NewtonBodyGetOmega(m_body0, &threadOmega[0]);
			NewtonBodyGetOmega(m_body1, &parentOmega[0]);

			NewtonBodyGetVelocity(m_body0, &threadVeloc[0]);
			NewtonBodyGetVelocity(m_body1, &parentVeloc[0]);

			NewtonBodyGetCentreOfMass(m_body0, &threadCOM[0]);
			NewtonBodyGetCentreOfMass(m_body1, &parentCOM[0]);

			NewtonBodyGetMatrix(m_body0, &threadMatrix[0][0]);
			NewtonBodyGetMatrix(m_body1, &parentMatrix[0][0]);

			threadCOM = threadMatrix.TransformVector(threadCOM);
			parentCOM = parentMatrix.TransformVector(parentCOM);
			
			ApplySuspesionForce (timestep,
				m_body0, m_rearHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
				m_body1, m_rearHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);

			ApplySuspesionForce (timestep,
				m_body0, m_frontHarpointOnThread, threadMatrix, threadCOM, threadVeloc, threadOmega,
				m_body1, m_frontHarpointOnParent, parentMatrix, parentCOM, parentVeloc, parentOmega);


			CustomSlidingContact::SubmitConstraints (timestep, threadIndex);
		}
Example #12
0
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
// set the transformation of a rigid body
void 
PhysicsActor::TransformCallback(const NewtonBody* _body, const Zen::Math::Real* _matrix)
{
    void* pBody = NewtonBodyGetUserData(_body);
    if (pBody != NULL)
    {
        PhysicsActor* pShape = static_cast<PhysicsActor*>(pBody);

        // Only use the transform callback if the state is active
        if (pShape->m_activationState != 0)
        {
            Math::Matrix4 transform;
            for(int x = 0; x < 16; x++)
            {
                transform.m_array[x] = _matrix[x];
            }
            TransformEventData evenData(*pShape, transform);
            pShape->onTransformEvent(evenData);
        }
    }

#if 0
    // HACK Keep the object to a constant velocity
    Math::Vector3 velocity;

    NewtonBodyGetVelocity(_body, velocity.m_array);

    velocity.normalize();
    velocity = velocity * 50;

    NewtonBodySetVelocity(_body, velocity.m_array);
#endif

    //std::cout << "TransformCallback()" << std::endl;
#if 0
	RenderPrimitive* primitive;

	// get the graphic object form the rigid body
	primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);

	// set the transformation matrix for this rigid body
	dMatrix& mat = *((dMatrix*)matrix);
	primitive->SetMatrix (mat);
#endif
}
Example #13
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));
}
void dCustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;
	dVector omega0(0.0f);
	dVector veloc1(0.0f);
	dFloat jacobian0[6];
	dFloat jacobian1[6];

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);
	
	// calculate the angular velocity for both bodies
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	NewtonBodyGetVelocity(m_body1, &veloc1[0]);
	dVector dir0 (matrix0.m_front.Scale (m_gearRatio));
	const dVector& dir1 = matrix1.m_front;

	jacobian0[0] = dFloat(0.0f);
	jacobian0[1] = dFloat(0.0f);
	jacobian0[2] = dFloat(0.0f);
	jacobian0[3] = dir0.m_x;
	jacobian0[4] = dir0.m_y;
	jacobian0[5] = dir0.m_z;

	jacobian1[0] = dir1.m_x;
	jacobian1[1] = dir1.m_y;
	jacobian1[2] = dir1.m_z;
	jacobian1[3] = dFloat(0.0f);
	jacobian1[4] = dFloat(0.0f);
	jacobian1[5] = dFloat(0.0f);

	dFloat w0 = omega0.DotProduct3(dir0);
	dFloat w1 = veloc1.DotProduct3(dir1);
	dFloat relOmega = w0 + w1;
	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
	dFloat relAccel = -0.5f * relOmega * invTimestep;
	NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
	NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void PhysicMap::playerForceAndTorque(const NewtonBody* nBody)
{
    int id = (int)(NewtonBodyGetUserData(nBody));
    CompLocation* loc = CKernel::data()->getGameObjectLocation(id);
    float fMasse, ixx, iyy, izz;
    NewtonBodyGetMassMatrix(nBody, &fMasse, &ixx, &iyy, &izz);

    // Apply gravity force
    Triplet_f gravity(0, -fMasse * 9.81f, 0);
    NewtonBodyAddForce(nBody, &gravity.x);

    // Apply movment force
    Triplet_f desiredVel = loc->desiredVel() * 20;
    Triplet_f currentVel;
    NewtonBodyGetVelocity(nBody,&currentVel.x);
	Triplet_f forceApply = (desiredVel - currentVel) * fMasse * 10;
	forceApply.y = 0;
    NewtonBodyAddForce(nBody, &forceApply.x);

    // Apply rotation
    Matrix_f mat;
    NewtonBodyGetMatrix(nBody, mat.raw());
    mat.setRotationY(loc->desiredAngle());
    NewtonBodySetMatrix(nBody, mat.raw());

    // Apply shot recoil
    if (loc->isImpact())
    {
        Triplet_f impact = loc->getNormal() * -3;
        NewtonAddBodyImpulse(nBody, &impact.x, &mat.position().x);
    }

    // Update the game object
    loc->setPosition(mat.position());
    loc->setDirection(Triplet_f(loc->desiredAngle()));
    loc->setVelocity(currentVel);
}
void dVehicleSingleBody::RigidBodyToStates()
{
	dVector vector;
	dMatrix matrix;
	dComplementaritySolver::dBodyState* const chassisBody = GetBody();

	// get data from engine rigid body and copied to the vehicle chassis body
	NewtonBodyGetMatrix(m_newtonBody, &matrix[0][0]);
	chassisBody->SetMatrix(matrix);

static int xxx;
xxx++;
if (xxx == 1500)
{
//	NewtonBodyGetVelocity(m_newtonBody, &vector[0]);
//	vector.m_x += 2.0f;
//	NewtonBodySetVelocity(m_newtonBody, &vector[0]);
}


	NewtonBodyGetVelocity(m_newtonBody, &vector[0]);
	chassisBody->SetVeloc(vector);

	NewtonBodyGetOmega(m_newtonBody, &vector[0]);
	chassisBody->SetOmega(vector);

	NewtonBodyGetForce(m_newtonBody, &vector[0]);
	chassisBody->SetForce(vector);

	NewtonBodyGetTorque(m_newtonBody, &vector[0]);
	chassisBody->SetTorque(vector);

	chassisBody->UpdateInertia();
	
	dVehicleInterface::RigidBodyToStates();
}
Example #17
0
	cVector3f cPhysicsBodyNewton::GetLinearVelocity() const
	{
		cVector3f vVel;
		NewtonBodyGetVelocity(m_pNewtonBody, vVel.v);
		return vVel;
	}
Example #18
0
void CustomSlider::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dMatrix matrix0;
	dMatrix matrix1;

	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix (matrix0, matrix1);

	// Restrict the movement on the pivot point along all two orthonormal axis direction perpendicular to the motion
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]);
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]);
	
 	// three rows to restrict rotation around around the parent coordinate system
	dFloat sinAngle;
	dFloat cosAngle;
	CalculatePitchAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_front[0]);

	CalculateYawAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_up[0]);

	CalculateRollAngle(matrix0, matrix1, sinAngle, cosAngle);
	NewtonUserJointAddAngularRow(m_joint, -dAtan2(sinAngle, cosAngle), &matrix1.m_right[0]);


	// calculate position and speed	
	dVector veloc0(0.0f, 0.0f, 0.0f, 0.0f); 
	dVector veloc1(0.0f, 0.0f, 0.0f, 0.0f);  
	if (m_body0) {
		NewtonBodyGetVelocity(m_body0, &veloc0[0]);
	}
	if (m_body1) {
		NewtonBodyGetVelocity(m_body1, &veloc1[0]);
	}
	m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix1.m_front;
	m_speed = (veloc0 - veloc1) % matrix1.m_front;

	// if limit are enable ...
	m_hitLimitOnLastUpdate = false;
	if (m_limitsOn) {
		if (m_posit < m_minDist) {
			// indicate that this row hit a limit
			m_hitLimitOnLastUpdate = true;

			// get a point along the up vector and set a constraint  
			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_minDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
			
		} else if (m_posit > m_maxDist) {
			// indicate that this row hit a limit
			m_hitLimitOnLastUpdate = true;

			// get a point along the up vector and set a constraint  

			const dVector& p0 = matrix0.m_posit;
			dVector p1 (p0 + matrix0.m_front.Scale (m_maxDist - m_posit));
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
			// allow the object to return but not to kick going forward
			NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);

		} else {

/*
			// uncomment this for a slider with friction

			// take any point on body0 (origin)
			const dVector& p0 = matrix0.m_posit;

			dVector veloc0; 
			dVector veloc1; 
			dVector omega1; 

			NewtonBodyGetVelocity(m_body0, &veloc0[0]);
			NewtonBodyGetVelocity(m_body1, &veloc1[0]);
			NewtonBodyGetOmega(m_body1, &omega1[0]);

			// this assumes the origin of the bodies the matrix pivot are the same
			veloc1 += omega1 * (matrix1.m_posit - p0);

			dFloat relAccel; 
			relAccel = ((veloc1 - veloc0) % matrix0.m_front) / timestep;

			#define MaxFriction 10.0f
			NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAccel);
			NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
			NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
		}
	} 
 }
static void CreateDebriPiece (const NewtonBody* sourceBody, NewtonMesh* mesh, dFloat volume)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;
	dFloat shapeVolume;
	NewtonWorld* world;
	NewtonBody* rigidBody;
	NewtonCollision* collision;
	OGLMesh* meshInstance;
	SceneManager* system;
	RenderPrimitive* primitive;
	dVector inertia;
	dVector origin;
	dVector veloc;
	dVector omega;
	dMatrix matrix;

	world = NewtonBodyGetWorld (sourceBody);

	NewtonBodyGetMatrix (sourceBody, &matrix[0][0]);

	NewtonBodyGetMassMatrix (sourceBody, &mass, &Ixx, &Iyy, &Izz);

	// make a visual object
	meshInstance = new OGLMesh();

	meshInstance->BuildFromMesh (mesh);

	// create a visual geometry
	primitive = new RenderPrimitive (matrix, meshInstance);
	meshInstance->Release();

	// save the graphics system
	system = (SceneManager*) NewtonWorldGetUserData(world);
	system->AddModel (primitive);

	collision = NewtonCreateConvexHullFromMesh (world, mesh, 0.1f, DEBRI_ID);

	// calculate the moment of inertia and the relative center of mass of the solid
	shapeVolume = NewtonConvexCollisionCalculateVolume (collision);
	NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);	

	mass = mass * shapeVolume / volume;
	Ixx = mass * inertia[0];
	Iyy = mass * inertia[1];
	Izz = mass * inertia[2];

	//create the rigid body
	rigidBody = NewtonCreateBody (world, collision);

	// set the correct center of gravity for this body
	NewtonBodySetCentreOfMass (rigidBody, &origin[0]);

	// set the mass matrix
	NewtonBodySetMassMatrix (rigidBody, mass, Ixx, Iyy, Izz);

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

	// assign the wood id
//	NewtonBodySetMaterialGroupID (rigidBody, NewtonBodyGetMaterialGroupID(source));

	// set continue collision mode
	NewtonBodySetContinuousCollisionMode (rigidBody, 1);

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

	// set the transform call back function
	NewtonBodySetTransformCallback (rigidBody, PhysicsSetTransform);

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

	// set the matrix for both the rigid body and the graphic body
	NewtonBodySetMatrix (rigidBody, &matrix[0][0]);
	PhysicsSetTransform (rigidBody, &matrix[0][0], 0);

	NewtonBodyGetVelocity(sourceBody, &veloc[0]);
	NewtonBodyGetOmega(sourceBody, &omega[0]);
	veloc += omega * matrix.RotateVector(origin);

// for now so that I can see the body
veloc = dVector (0, 0, 0, 0);
//	omega = dVector (0, 0, 0, 0);

	NewtonBodySetVelocity(rigidBody, &veloc[0]);
	NewtonBodySetOmega(rigidBody, &omega[0]);

	NewtonReleaseCollision(world, collision);

}
		void ApplyTracktionForce (dFloat timestep, const NewtonBody* track)
		{
			dVector veloc;
			dVector omega;
			dMatrix matrix;

			NewtonBodyGetOmega(m_body0, &omega[0]);
			NewtonBodyGetVelocity(m_body0, &veloc[0]);
			NewtonBodyGetMatrix (m_body0, &matrix[0][0]);
			

			// itetate over the contact list and condition each contact direction anc contact acclerations
			for (NewtonJoint* contactJoint = NewtonBodyGetFirstContactJoint (track); contactJoint; contactJoint = NewtonBodyGetNextContactJoint (track, contactJoint)) {
				_ASSERTE ((NewtonJointGetBody0 (contactJoint) == track) || (NewtonJointGetBody1 (contactJoint) == track));

				#ifdef REMOVE_REDUNDAT_CONTACT	
				int contactCount;
				contactCount = NewtonContactJointGetContactCount(contactJoint);
				if (contactCount > 2) {
					// project the contact to the bounday of the conve hull o fteh trhread foot ptint 
					dFloat maxDist;
					dFloat minDist;
					void* minContact;
					void* maxContact;
					
					dMatrix matrix;
			
					minContact = NULL;
				    maxContact = NULL;
					NewtonBodyGetMatrix (track, &matrix[0][0]);

					maxDist = -1.0e10f;
					minDist = -1.0e10f;
					//find the best two contacts and remove all others
					for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
						dFloat dist;
						dVector point;
						dVector normal;
						NewtonMaterial* material;

					    material = NewtonContactGetMaterial (contact);
						NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
						
						dist = matrix.m_front % point;
						if (dist > maxDist) {
							maxDist = dist;
							maxContact = contact;
						} 
						if (-dist > minDist) {
							minDist = -dist;
							minContact = contact;
						}
						
					}

					// now delete all reduntact contacts
					void* nextContact;
					NewtonWorld* world;

					world = NewtonBodyGetWorld (track);
					NewtonWorldCriticalSectionLock(world);
					for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = nextContact) {
						nextContact = NewtonContactJointGetNextContact (contactJoint, contact);
						if (!((contact == minContact) || (contact == maxContact))) {
							NewtonContactJointRemoveContact (contactJoint, contact);
						}
					}
					NewtonWorldCriticalSectionUnlock(world);
				}

				#endif

			
				for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
					dFloat speed;
					dFloat accel;
					dVector point;
					dVector normal;
					dVector dir0;
					dVector dir1;
					NewtonMaterial* material;

				    material = NewtonContactGetMaterial (contact);
					NewtonMaterialContactRotateTangentDirections (material, &matrix.m_front[0]);
					NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
					NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir1[0]);


					dVector posit (point - matrix.m_posit);
					veloc += omega * posit;
					speed = veloc % dir0;
				//	accel = m_accel - 0.1f * speed + (((posit % m_matrix.m_right) > 0.0f) ? m_turnAccel : - m_turnAccel);
					accel = m_veloc + (((posit % matrix.m_right) > 0.0f) ? m_turnVeloc : - m_turnVeloc);

					accel = (accel - speed) * 0.5f / timestep;

			//		NewtonMaterialSetContactStaticFrictionCoef (material, 1.0f, 0);
			//		NewtonMaterialSetContactKineticFrictionCoef (material, 1.0f, 0);
					NewtonMaterialSetContactFrictionCoef (material, 1.0f, 1.0f, 0);

			//		NewtonMaterialSetContactStaticFrictionCoef (material, 0.5f, 1);
			//		NewtonMaterialSetContactKineticFrictionCoef (material, 0.5f, 1);
					NewtonMaterialSetContactFrictionCoef (material, 0.5f, 0.5f, 1);
					
					NewtonMaterialSetContactTangentAcceleration (material, accel, 0);
				}

				// for debug purpose show the contact
				ShowJointContacts (contactJoint);
			}
		}
Example #21
0
	void Debug () const 
	{
		NewtonBody* const body = m_controller->GetBody();
		const CustomVehicleControllerBodyStateChassis& chassis = m_controller->GetChassisState ();

		dFloat scale = -4.0f / (chassis.GetMass() * DEMO_GRAVITY);
		dVector p0 (chassis.GetCenterOfMass());

		glDisable (GL_LIGHTING);
		glDisable(GL_TEXTURE_2D);

		glLineWidth(3.0f);
		glBegin(GL_LINES);


		// draw vehicle weight at the center of mass
		dFloat lenght = scale * chassis.GetMass() * DEMO_GRAVITY;
		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f (p0.m_x, p0.m_y, p0.m_z);
		glVertex3f (p0.m_x, p0.m_y - lenght, p0.m_z);

		// draw vehicle front dir
		glColor3f(1.0f, 1.0f, 1.0f);
		dVector r0 (p0 + chassis.GetMatrix()[1].Scale (0.5f));
		dVector r1 (r0 + chassis.GetMatrix()[0].Scale (1.0f));
		glVertex3f (r0.m_x, r0.m_y, r0.m_z);
		glVertex3f (r1.m_x, r1.m_y, r1.m_z);


		// draw the velocity vector, a little higher so that is not hidden by the vehicle mesh 
		dVector veloc;
		NewtonBodyGetVelocity(body, &veloc[0]);
		dVector q0 (p0 + chassis.GetMatrix()[1].Scale (1.0f));
		dVector q1 (q0 + veloc.Scale (0.25f));
		glColor3f(1.0f, 1.0f, 0.0f);
		glVertex3f (q0.m_x, q0.m_y, q0.m_z);
		glVertex3f (q1.m_x, q1.m_y, q1.m_z);
		
		for (CustomVehicleControllerBodyStateTire* node = m_controller->GetFirstTire(); node; node = m_controller->GetNextTire(node)) {
			const CustomVehicleControllerBodyStateTire& tire = *node;
			dVector p0 (tire.GetCenterOfMass());

const dMatrix& tireMatrix = tire.GetLocalMatrix ();
p0 += chassis.GetMatrix()[2].Scale ((tireMatrix.m_posit.m_z > 0.0f ? 1.0f : -1.0f) * 0.25f);

			// draw the tire load 
			dVector p1 (p0 + tire.GetTireLoad().Scale (scale));
			glColor3f (0.0f, 0.0f, 1.0f);
			glVertex3f (p0.m_x, p0.m_y, p0.m_z);
			glVertex3f (p1.m_x, p1.m_y, p1.m_z);

			// show tire lateral force
			dVector p2 (p0 - tire.GetLateralForce().Scale (scale));
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f (p0.m_x, p0.m_y, p0.m_z);
			glVertex3f (p2.m_x, p2.m_y, p2.m_z);

			// show tire longitudinal force
			dVector p3 (p0 - tire.GetLongitudinalForce().Scale (scale));
			glColor3f (0.0f, 1.0f, 0.0f);
			glVertex3f (p0.m_x, p0.m_y, p0.m_z);
			glVertex3f (p3.m_x, p3.m_y, p3.m_z);
		}

		glEnd();

		glLineWidth(1.0f);

	}
	void SimulationLister(DemoEntityManager* const scene, DemoEntityManager::dListNode* const mynode, dFloat timeStep)
	{
		m_delay --;
		if (m_delay > 0) {
			return;
		}

		// see if the net force on the body comes fr a high impact collision
		dFloat maxInternalForce = 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 point;
				//dVector normal;	
				dVector contactForce;
				NewtonMaterial* const material = NewtonContactGetMaterial (contact);
				//NewtonMaterialGetContactPositionAndNormal (material, &point.m_x, &normal.m_x);
				NewtonMaterialGetContactForce(material, m_myBody, &contactForce[0]);
				dFloat forceMag = contactForce % contactForce;
				if (forceMag > maxInternalForce) {
					maxInternalForce = forceMag;
				}
			}
		}

		

		// if the force is bigger than 4 Gravities, It is considered a collision force
		dFloat maxForce = BREAK_FORCE_IN_GRAVITIES * m_myweight;

		if (maxInternalForce > (maxForce * maxForce)) {
			NewtonWorld* const world = NewtonBodyGetWorld(m_myBody);

			dFloat Ixx; 
			dFloat Iyy; 
			dFloat Izz; 
			dFloat mass; 
			NewtonBodyGetMassMatrix(m_myBody, &mass, &Ixx, &Iyy, &Izz);

			dVector com;
			dVector veloc;
			dVector omega;
			dMatrix bodyMatrix;

			NewtonBodyGetVelocity(m_myBody, &veloc[0]);
			NewtonBodyGetOmega(m_myBody, &omega[0]);
			NewtonBodyGetCentreOfMass(m_myBody, &com[0]);
			NewtonBodyGetMatrix(m_myBody, &bodyMatrix[0][0]);
			com = bodyMatrix.TransformVector (com);

			dMatrix matrix (GetCurrentMatrix());
			dQuaternion rotation (matrix);
			for (ShatterEffect::dListNode* node = m_effect.GetFirst(); node; node = node->GetNext()) {
				ShatterAtom& atom = node->GetInfo();

				DemoEntity* const entity = new DemoEntity (NULL);
				entity->SetMesh (atom.m_mesh);
				entity->SetMatrix(*scene, rotation, matrix.m_posit);
				entity->InterpolateMatrix (*scene, 1.0f);
				scene->Append(entity);

				int materialId = 0;

				dFloat debriMass = mass * atom.m_massFraction;
				dFloat Ixx = debriMass * atom.m_momentOfInirtia.m_x;
				dFloat Iyy = debriMass * atom.m_momentOfInirtia.m_y;
				dFloat Izz = debriMass * atom.m_momentOfInirtia.m_z;

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

				// set the correct center of gravity for this body
				NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]);

				// calculate the center of mas of the debris
				dVector center (matrix.TransformVector(atom.m_centerOfMass));

				// calculate debris initial velocity
				dVector v (veloc + omega * (center - com));

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

				// set the  debrie center of mass
				NewtonBodySetCentreOfMass (rigidBody, &atom.m_centerOfMass[0]);


				// set the mass matrix
				NewtonBodySetMassMatrix (rigidBody, debriMass, Ixx, Iyy, Izz);

				// activate 
				//	NewtonBodyCoriolisForcesMode (blockBoxBody, 1);

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

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

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

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

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

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

			NewtonDestroyBody(world, m_myBody);
			scene->RemoveEntity	(mynode);
		}
	};
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex)
{

	// check if this is an impulsive time step
	
	if (timestep > 0.0f) {
		dMatrix matrix0;
		dVector v(0.0f);
		dVector w(0.0f);
		dVector cg(0.0f);

		dFloat invTimestep = 1.0f / timestep;

		// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
		NewtonBodyGetOmega (m_body0, &w[0]);
		NewtonBodyGetVelocity (m_body0, &v[0]);
		NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
		NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);

		dVector p0 (matrix0.TransformVector (m_localHandle));

		dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg));
		dVector relPosit (m_targetPosit - p0);
		dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
		dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); 
			
		// Restrict the movement on the pivot point along all tree orthonormal direction
		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]);
		NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right);
		NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
		NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

		if (m_pickMode) {
			dQuaternion rotation;

			NewtonBodyGetRotation (m_body0, &rotation.m_q0);
			if (m_targetRot.DotProduct (rotation) < 0.0f) {
				rotation.m_q0 *= -1.0f; 
				rotation.m_q1 *= -1.0f; 
				rotation.m_q2 *= -1.0f; 
				rotation.m_q3 *= -1.0f; 
			}

			dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w);
			dFloat mag = relOmega % relOmega;
			if (mag > 1.0e-6f) {
				dVector pin (relOmega.Scale (1.0f / mag));
				dMatrix basis (dGrammSchmidt (pin)); 	
				dFloat relSpeed = dSqrt (relOmega % relOmega);
				dFloat relAlpha = relSpeed * invTimestep;

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

			} else {

				dVector relAlpha (w.Scale (-invTimestep));
				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

				NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
				NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
				NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
				NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
			}

		} else {
			// this is the single handle pick mode, add some angular friction

			dVector relAlpha = w.Scale (-invTimestep);
			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);

			NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
			NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right);
			NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f);
			NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * 0.025f);
		}
	}
}
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
	dMatrix matrix; 
	dQuaternion bodyRotation;
	dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); 
	dVector omega(0.0f, 0.0f, 0.0f, 0.0f);  

	CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
	NewtonWorld* const world = manager->GetWorld();

	// apply the player motion, by calculation the desired plane linear and angular velocity
	manager->ApplyPlayerMove (this, timestep);

	// get the body motion state 
	NewtonBodyGetMatrix(m_body, &matrix[0][0]);
	NewtonBodyGetVelocity(m_body, &veloc[0]);
	NewtonBodyGetOmega(m_body, &omega[0]);

	// integrate body angular velocity
	NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); 
	bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
	matrix = dMatrix (bodyRotation, matrix.m_posit);

	// integrate linear velocity
	dFloat normalizedTimeLeft = 1.0f; 
	dFloat step = timestep * dSqrt (veloc % veloc) ;
	dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
	int prevContactCount = 0;
	CustomControllerConvexCastPreFilter castFilterData (m_body);
	NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];

	dVector updir (matrix.RotateVector(m_upVector));

	dVector scale;
	NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
	//const dFloat radio = m_outerRadio * 4.0f;
	const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
	NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);


	NewtonWorldConvexCastReturnInfo upConstratint;
	memset (&upConstratint, 0, sizeof (upConstratint));
	upConstratint.m_normal[0] = m_upVector.m_x;
	upConstratint.m_normal[1] = m_upVector.m_y;
	upConstratint.m_normal[2] = m_upVector.m_z;
	upConstratint.m_normal[3] = m_upVector.m_w;

	for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
		if ((veloc % veloc) < 1.0e-6f) {
			break;
		}

		dFloat timetoImpact;
		NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
		dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
		int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
		if (contactCount) {
			contactCount = manager->ProcessContacts (this, info, contactCount);
		}

		if (contactCount) {
			matrix.m_posit += veloc.Scale (timetoImpact * timestep);
			if (timetoImpact > 0.0f) {
				matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; 
			}

			normalizedTimeLeft -= timetoImpact;

			dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
			dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];

			for (int i = 1; i < contactCount; i ++) {
				dVector n0 (info[i-1].m_normal);
				for (int j = 0; j < i; j ++) {
					dVector n1 (info[j].m_normal);
					if ((n0 % n1) > 0.9999f) {
						info[i] = info[contactCount - 1];
						i --;
						contactCount --;
						break;
					}
				}
			}

			int count = 0;
			if (!m_isJumping) {
				upConstratint.m_point[0] = matrix.m_posit.m_x;
				upConstratint.m_point[1] = matrix.m_posit.m_y;
				upConstratint.m_point[2] = matrix.m_posit.m_z;
				upConstratint.m_point[3] = matrix.m_posit.m_w;

				speed[count] = 0.0f;
				bounceNormal[count] = dVector (upConstratint.m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
				count ++;
			}

			for (int i = 0; i < contactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (info[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
				count ++;
			}

			for (int i = 0; i < prevContactCount; i ++) {
				speed[count] = 0.0f;
				bounceNormal[count] = dVector (prevInfo[i].m_normal);
				bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
				count ++;
			}

			dFloat residual = 10.0f;
			dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
				residual = 0.0f;
				for (int k = 0; k < count; k ++) {
					dVector normal (bounceNormal[k]);
					dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
					dFloat x = speed[k] + v;
					if (x < 0.0f) {
						v = 0.0f;
						x = 0.0f;
					}

					if (dAbs (v) > residual) {
						residual = dAbs (v);
					}

					auxBounceVeloc += normal.Scale (x - speed[k]);
					speed[k] = x;
				}
			}

			dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
			for (int i = 0; i < count; i ++) {
				dVector normal (bounceNormal[i]);
				velocStep += normal.Scale (speed[i]);
			}
			veloc += velocStep;

			dFloat velocMag2 = velocStep % velocStep;
			if (velocMag2 < 1.0e-6f) {
				dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
				matrix.m_posit += veloc.Scale (advanceTime);
				normalizedTimeLeft -= advanceTime / timestep;
			}

			prevContactCount = contactCount;
			memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));

		} else {
			matrix.m_posit = destPosit;
			matrix.m_posit.m_w = 1.0f;
			break;
		}
	}
	NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);

	// determine if player is standing on some plane
	dMatrix supportMatrix (matrix);
	supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
	if (m_isJumping) {
		dVector dst (matrix.m_posit);
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	} else {
		step = dAbs (updir % veloc.Scale (timestep));
		dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
		dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
		UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
	}

	// set player velocity, position and orientation
	NewtonBodySetVelocity(m_body, &veloc[0]);
	NewtonBodySetMatrix (m_body, &matrix[0][0]);
}
Example #25
0
void CustomDGRayCastCar::IntegrateTires (dFloat timestep, int threadIndex)
{
	dMatrix bodyMatrix;  

	// get the vehicle global matrix, and use it in several calculations
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyOmega;
	dVector bodyVelocity;
	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

	// set the current vehicle speed
	m_curSpeed = bodyMatrix.m_front % bodyVelocity;

	for (int i = 0; i < m_tiresCount; i ++ ) {
		Tire& tire = m_tires[i];
/*
		if (tire.m_tireIsConstrained) {
			// the torqued generate by the tire can no be larger than the external torque on the tire 
			// when this happens ther tire is spinning unde contrained rotation 

			// V % dir + W * R % dir = 0
			// where V is the tire Axel velocity
			// W is the tire local angular velocity
			// R is the tire radius
			// dir is the longitudinal direction of of the tire.

			dFloat contactRadius;
			dFloat axelLinealSpeed;
			dVector tireAxelPosit (chassisMatrix.TransformVector (tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit)));
			dVector tireAxelVeloc = bodyVelocity + bodyOmega * (tireAxelPosit - chassisMatrix.m_posit) - tire.m_hitBodyPointVelocity; 
			axelLinealSpeed = tireAxelVeloc % chassisMatrix.m_front;


			dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
			contactRadius = (tire.m_lateralPin * tireRadius) % tire.m_longitudinalPin;
			tire.m_angularVelocity = - axelLinealSpeed / contactRadius ;

		} else if (tire.m_tireIsOnAir) {
			if (tire.m_breakForce > 1.0e-3f) {
				tire.m_angularVelocity = 0.0f;
			} else {
				//the tire is on air, need to be integrate net toque and apply a drag coneficenct

	//			dFloat nettorque = tire.m_angularVelocity;
	//			// this checkup is suposed to fix a infinit division by zero...
	//			if ( dAbs(tireContactSpeed)  > 1.0e-3) { 
	//				nettorque = - (tireLinearSpeed) / (tireContactSpeed);
	//			} 
				//tire.m_angularVelocity = - tireLinearSpeed / tireContactSpeed;
				dFloat torque;
				torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
				tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
			}
		} else {
			// there is a next torque on the tire
			dFloat torque;
			torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
			tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
		}
*/
		if (tire.m_tireIsOnAir) {
			if (tire.m_breakForce > 1.0e-3f) {
				tire.m_angularVelocity = 0.0f;
			} else {
				//the tire is on air, need to be integrate net toque and apply a drag coeficient
				dFloat torque;
				torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
				tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
			}
		} else if (tire.m_tireIsConstrained) {
			// the torqued generate by the tire can no be larger than the external torque on the tire 
			// when this happens there tire is spinning under constrained rotation 

			// V % dir + W * R % dir = 0
			// where V is the tire Axel velocity
			// W is the tire local angular velocity
			// R is the tire radius
			// dir is the longitudinal direction of of the tire.

			dFloat contactRadius;
			dFloat axelLinealSpeed;
			dVector tireAxelPosit (chassisMatrix.TransformVector (tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit)));
			dVector tireAxelVeloc = bodyVelocity + bodyOmega * (tireAxelPosit - chassisMatrix.m_posit) - tire.m_hitBodyPointVelocity; 
			axelLinealSpeed = tireAxelVeloc % chassisMatrix.m_front;


			dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
			contactRadius = (tire.m_lateralPin * tireRadius) % tire.m_longitudinalPin;
			tire.m_angularVelocity = - axelLinealSpeed / contactRadius ;

		} else {

			// there is a next torque on the tire
			dFloat torque;
			torque = tire.m_torque - tire.m_angularVelocity * tire.m_Ixx * TIRE_VISCUOS_DAMP;
			tire.m_angularVelocity += torque * tire.m_IxxInv * timestep;
		}


		// spin the tire by the angular velocity
		tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);

		// reset the tire torque
		tire.m_torque = 0.0f;
		tire.m_breakForce = 0.0f;  
	}
}
Example #26
0
void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep)
{
	dVector com; 
	dMatrix matrix; 
	dVector omega0;
	dVector veloc0;
	dVector omega1;
	dVector veloc1;
	dVector pointVeloc;

	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, 0.0f, 0.0f, 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;
	NewtonBodyGetMassMatrix (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);
}
Example #27
0
Ogre::Vector3 Body::getVelocity()
{
	Ogre::Vector3 ret;
	NewtonBodyGetVelocity( m_body, &ret.x );
	return ret;
}
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{

	// get the simulation time
//	dFloat invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	dMatrix bodyMatrix;  
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyForce;
	dVector bodyOmega;
	dVector bodyVelocity;


	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

//static int xxx;
//dTrace (("frame %d veloc(%f %f %f)\n", xxx, bodyVelocity[0], bodyVelocity[1], bodyVelocity[2]));
//xxx ++;
//if (xxx >= 210) {
//xxx *=1;
//bodyVelocity.m_x = 0;
//bodyVelocity.m_z = 10;
//NewtonBodySetVelocity (m_body0, &bodyVelocity[0]);
//}

//	dVector normalForces (0.0f, 0.0f, 0.0f, 0.0f);
	// all tire is on air check
	m_vehicleOnAir = 0;
//	int constraintIndex = 0;
	for (int i = 0; i < m_tiresCount; i ++) {

//		dTrace (("tire: %d ", i));

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
//		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPositGlobal = chassisMatrix.TransformVector (tire.m_harpointInJointSpace - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVelocGlobal = bodyVelocity + bodyOmega * (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit); 
		tire.m_lateralPinGlobal = chassisMatrix.RotateVector (tire.m_localAxisInJointSpace);
		tire.m_longitudinalPinGlobal = chassisMatrix.m_up * tire.m_lateralPinGlobal;

		if (tire.m_posit < tire.m_suspensionLenght )  {

			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 


			// calculate the relative velocity
			dVector tireHubVeloc (tire.m_tireAxelVelocGlobal - tire.m_hitBodyPointVelocity);
			dFloat suspensionSpeed = - (tireHubVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			dFloat distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

//			dTrace (("(load = %f) ", tire.m_tireLoad));


			//tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);


			// calculate relative velocity at the tire center
			//dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			//axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;
			dFloat axelLinearSpeed = tireHubVeloc % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
			//dVector tireAngularVelocity (tire.m_lateralPinGlobal.Scale (tire.m_angularVelocity));
			//dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPositGlobal);
			//dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);	


			// calculate slip ratio and max longitudinal force
			//dFloat tireRotationSpeed = -(tireRotationalVelocityAtContact % tire.m_longitudinalPinGlobal);
			//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;

			//dTrace (("(slipRatio = %f) ", slipRatioCoef));

			// calculate the formal longitudinal force the tire apply to the chassis
			//dFloat longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;

			dFloat longitudinalForceMag = CalculateLongitudinalForce (i, axelLinearSpeed, tire.m_tireLoad * tire.m_groundFriction);

//			dTrace (("(longForce = %f) ", longitudinalForceMag));

#if 0

			// now calculate relative velocity a velocity at contact point
			//dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact); 
			//dVector tireContactAbsoluteVelocity (tireHubVeloc + tireRotationalVelocityAtContact); 

			// calculate the side slip as the angle between the tire lateral speed and longitudinal speed 
			//dFloat lateralSpeed = tireContactRelativeVelocity % tire.m_lateralPin;
			dFloat lateralSpeed = tireHubVeloc % tire.m_lateralPinGlobal;

			dFloat sideSlipCoef = dAtan2 (dAbs (lateralSpeed), dAbs (axelLinearSpeed));
			dFloat lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;

			// Apply brake, need some little fix here.
			// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
			if ( tire.m_breakForce > 1.0e-3f ) {
				_ASSERTE (0);
/*
				// row constrained force is save for later determine the dynamic state of this tire 
  				tire.m_isBrakingForceIndex = constraintIndex;
				constraintIndex ++;

				frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
				if (tire.m_breakForce > frictionCircleMag) {
					tire.m_breakForce = frictionCircleMag;
				}

				//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
				NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
				NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
				NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);

				// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
				tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
				if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
  					lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
				}
*/
			} 


			//project the longitudinal and lateral forces over the circle of friction for this tire; 
			dFloat frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;

			dFloat tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
			if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
				dFloat invMag2;
				invMag2 = frictionCircleMag / dSqrt (tireForceMag);
				longitudinalForceMag *= invMag2;
				lateralFrictionForceMag *= invMag2;
			}


			// submit this constraint for calculation of side slip forces
			lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
			tire.m_lateralForceIndex = constraintIndex;
			constraintIndex ++;
			NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPositGlobal[0], &tire.m_tireAxelPositGlobal[0], &tire.m_lateralPinGlobal[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);
#endif

			// accumulate the longitudinal force
			dVector tireForce (tire.m_longitudinalPinGlobal.Scale (longitudinalForceMag));
			tire.m_tireForceAcc += tireForce;

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector r (tire.m_tireAxelPositGlobal - chassisMatrix.m_posit);

			// add the toque the tire asserts on the car body (principle of action reaction)
			dVector torque (r * tire.m_tireForceAcc - tire.m_lateralPinGlobal.Scale (tire.m_torque));
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );
/*
			// calculate the net torque on the tire
			dFloat tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPinGlobal);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force cannot be larger than the applied engine torque 
				// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
				tire.m_tireIsConstrained = 1;
				tireTorqueMag = tire.m_torque;
			}

			tire.m_torque -= tireTorqueMag;
*/
//			normalForces += tire.m_tireForceAcc;

		} else {

			// there is a next torque on the tire
			tire.m_torque -= tire.m_angularVelocity * tire.m_Ixx * DG_TIRE_VISCUOS_DAMP;
			tire.m_angularVelocity += tire.m_torque * tire.m_IxxInv * timestep;
			if (m_tires[i].m_breakForce > dFloat (0.1f)) {
				tire.m_angularVelocity = 0.0f;
			}
		}

//		dTrace (("(tireTorque = %f) ", tire.m_torque));

		// spin the tire by the angular velocity
		tire.m_spinAngle = dMod (tire.m_spinAngle + tire.m_angularVelocity * timestep, 3.14159265f * 2.0f);

		// reset the tire torque
		tire.m_torque = 0.0f;
		tire.m_breakForce = 0.0f;  

//		dTrace (("\n"));

	}


	// add a row to simulate the engine rolling resistance
//	float bodyWeight = dAbs (normalForces % chassisMatrix.m_up) * m_rollingResistance;
//	if (bodyWeight > (1.0e-3f) * m_mass) {
//		NewtonUserJointAddLinearRow (m_joint, &chassisMatrix.m_posit[0], &chassisMatrix.m_posit[0], &chassisMatrix.m_front[0]);
//		NewtonUserJointSetRowMaximumFriction( m_joint,  bodyWeight);
//		NewtonUserJointSetRowMinimumFriction( m_joint, -bodyWeight);
//	}
}
Example #29
0
void CustomDGRayCastCar::SubmitConstraints (dFloat timestep, int threadIndex)
{
	int constraintIndex;
	dFloat invTimestep;
	dMatrix bodyMatrix;  

	// get the simulation time
	invTimestep = 1.0f / timestep ;

	// get the vehicle global matrix, and use it in several calculations
	NewtonBodyGetMatrix (m_body0, &bodyMatrix[0][0]);
	dMatrix chassisMatrix (m_localFrame * bodyMatrix);

	// get the chassis instantaneous linear and angular velocity in the local space of the chassis
	dVector bodyOmega;
	dVector bodyVelocity;
	
	NewtonBodyGetVelocity (m_body0, &bodyVelocity[0]);
	NewtonBodyGetOmega (m_body0, &bodyOmega[0]);

	// all tire is on air check
	m_vehicleOnAir = 0;
	constraintIndex = 0;
	for ( int i = 0; i < m_tiresCount; i ++ ) {

		Tire& tire = m_tires[i];
		tire.m_tireIsOnAir = 1;
		tire.m_tireIsConstrained = 0;	
		tire.m_tireForceAcc = dVector(0.0f, 0.0f, 0.0f, 0.0f);

		// calculate all suspension matrices in global space and tire collision
		dMatrix suspensionMatrix (CalculateSuspensionMatrix (i, 0.0f) * chassisMatrix);

		// calculate the tire collision
		CalculateTireCollision (tire, suspensionMatrix, threadIndex);

		// calculate the linear velocity of the tire at the ground contact
		tire.m_tireAxelPosit = chassisMatrix.TransformVector( tire.m_harpoint - m_localFrame.m_up.Scale (tire.m_posit));
		tire.m_tireAxelVeloc = bodyVelocity + bodyOmega * (tire.m_tireAxelPosit - chassisMatrix.m_posit); 
		tire.m_lateralPin = ( chassisMatrix.RotateVector ( tire.m_localAxis ) );
		tire.m_longitudinalPin = ( chassisMatrix.m_up * tire.m_lateralPin );

		if (tire.m_posit < tire.m_suspensionLenght )  {
			dFloat distance;
			dFloat sideSlipCoef;
			dFloat slipRatioCoef;
			dFloat tireForceMag;
			dFloat tireTorqueMag;
			dFloat suspensionSpeed;
			dFloat axelLinealSpeed;
			dFloat tireRotationSpeed;
			dFloat frictionCircleMag;
			dFloat longitudinalForceMag;
			dFloat lateralFrictionForceMag;


			tire.m_tireIsOnAir = 0;
			tire.m_hitBodyPointVelocity = dVector (0.0f, 0.0f, 0.0f, 1.0f);
			if (tire.m_HitBody){
				dMatrix matrix;
				dVector com;
				dVector omega;

				NewtonBodyGetOmega (tire.m_HitBody, &omega[0]);
				NewtonBodyGetMatrix (tire.m_HitBody, &matrix[0][0]);
				NewtonBodyGetCentreOfMass (tire.m_HitBody, &com[0]);
				NewtonBodyGetVelocity (tire.m_HitBody, &tire.m_hitBodyPointVelocity[0]);
				tire.m_hitBodyPointVelocity += (tire.m_contactPoint - matrix.TransformVector (com)) * omega;
			} 

			// calculate the relative velocity
			dVector relVeloc (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity);
			suspensionSpeed = - (relVeloc % chassisMatrix.m_up);

			// now calculate the tire load at the contact point
			// Tire suspension distance and hard limit.
			distance = tire.m_suspensionLenght - tire.m_posit;
			_ASSERTE (distance <= tire.m_suspensionLenght);
			tire.m_tireLoad = - NewtonCalculateSpringDamperAcceleration (timestep, tire.m_springConst, distance, tire.m_springDamper, suspensionSpeed );
			if ( tire.m_tireLoad < 0.0f ) {
				// since the tire is not a body with real mass it can only push the chassis.
				tire.m_tireLoad = 0.0f;
			} 

			//this suspension is applying a normalize force to the car chassis, need to scales by the mass of the car
			tire.m_tireLoad *= (m_mass * 0.5f);

			tire.m_tireIsConstrained = (dAbs (tire.m_torque) < 0.3f);

			// convert the tire load force magnitude to a torque and force.
			// accumulate the force doe to the suspension spring and damper
			tire.m_tireForceAcc += chassisMatrix.m_up.Scale (tire.m_tireLoad);

			// calculate relative velocity at the tire center
			dVector tireAxelRelativeVelocity (tire.m_tireAxelVeloc - tire.m_hitBodyPointVelocity); 

			// axle linear speed
			axelLinealSpeed = tireAxelRelativeVelocity % chassisMatrix.m_front;

			// calculate tire rotation velocity at the tire radio
			dVector tireAngularVelocity (tire.m_lateralPin.Scale (tire.m_angularVelocity));
			dVector tireRadius (tire.m_contactPoint - tire.m_tireAxelPosit);
			dVector tireRotationalVelocityAtContact (tireAngularVelocity * tireRadius);	

			longitudinalForceMag = 0.0f;
//			if (!tire.m_tireIsConstrained) {
				
				// calculate slip ratio and max longitudinal force
				tireRotationSpeed = tireRotationalVelocityAtContact % tire.m_longitudinalPin;
				slipRatioCoef = (dAbs (axelLinealSpeed) > 1.e-3f) ? ((-tireRotationSpeed - axelLinealSpeed) / dAbs (axelLinealSpeed)) : 0.0f;

				// calculate the formal longitudinal force the tire apply to the chassis
				longitudinalForceMag = m_normalizedLongitudinalForce.GetValue (slipRatioCoef) * tire.m_tireLoad * tire.m_groundFriction;
//			} 

		
			// now calculate relative velocity a velocity at contact point
			dVector tireContactRelativeVelocity (tireAxelRelativeVelocity + tireRotationalVelocityAtContact); 

			// calculate the sideslip as the angle between the tire lateral speed and longitudila speed 
			sideSlipCoef = dAtan2 (dAbs (tireContactRelativeVelocity % tire.m_lateralPin), dAbs (axelLinealSpeed));
			lateralFrictionForceMag = m_normalizedLateralForce.GetValue (sideSlipCoef) * tire.m_tireLoad * tire.m_groundFriction;

			// Apply brake, need some little fix here.
			// The fix is need to generate axial force when the brake is apply when the vehicle turn from the steer or on sliding.
			if ( tire.m_breakForce > 1.0e-3f ) {
				// row constrained force is save for later determine the dynamic state of this tire 
  				tire.m_isBrakingForceIndex = constraintIndex;
				constraintIndex ++;

				frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
				if (tire.m_breakForce > frictionCircleMag) {
					tire.m_breakForce = frictionCircleMag;
				}

				//NewtonUserJointAddLinearRow ( m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &chassisMatrix.m_front.m_x  );
				NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_longitudinalPin.m_x);
				NewtonUserJointSetRowMaximumFriction( m_joint, tire.m_breakForce);
				NewtonUserJointSetRowMinimumFriction( m_joint, -tire.m_breakForce);

				// there is a longitudinal force that will reduce the lateral force, we need to recalculate the lateral force
				tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + tire.m_breakForce * tire.m_breakForce;
				if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
  					lateralFrictionForceMag *= 0.25f * frictionCircleMag / dSqrt (tireForceMag);
				}
			} 


			//project the longitudinal and lateral forces over the circle of friction for this tire; 
			frictionCircleMag = tire.m_tireLoad * tire.m_groundFriction;
			tireForceMag = lateralFrictionForceMag * lateralFrictionForceMag + longitudinalForceMag * longitudinalForceMag;
			if (tireForceMag > (frictionCircleMag * frictionCircleMag)) {
				dFloat invMag2;
				invMag2 = frictionCircleMag / dSqrt (tireForceMag);
				longitudinalForceMag *= invMag2;
				lateralFrictionForceMag *= invMag2;
			}

			// submit this constraint for calculation of side slip forces
			lateralFrictionForceMag = dAbs (lateralFrictionForceMag);
			tire.m_lateralForceIndex = constraintIndex;
			constraintIndex ++;
			NewtonUserJointAddLinearRow (m_joint, &tire.m_tireAxelPosit[0], &tire.m_tireAxelPosit[0], &tire.m_lateralPin[0]);
			NewtonUserJointSetRowMaximumFriction (m_joint,  lateralFrictionForceMag);
			NewtonUserJointSetRowMinimumFriction (m_joint, -lateralFrictionForceMag);

			// accumulate the longitudinal force
			dVector tireForce (tire.m_longitudinalPin.Scale (longitudinalForceMag));
			tire.m_tireForceAcc += tireForce;

			// now we apply the combined tire force generated by this tire, to the car chassis
			dVector torque ((tire.m_tireAxelPosit - chassisMatrix.m_posit) * tire.m_tireForceAcc);
			NewtonBodyAddForce (m_body0, &tire.m_tireForceAcc[0]);
			NewtonBodyAddTorque( m_body0, &torque[0] );


			// calculate the net torque on the tire
			tireTorqueMag = -((tireRadius * tireForce) % tire.m_lateralPin);
			if (dAbs (tireTorqueMag) > dAbs (tire.m_torque)) {
				// the tire reaction force can no be larger than the applied engine torque 
				// when this happens the net torque is zero and the tire is constrained to the vehicle linear motion
				tire.m_tireIsConstrained = 1;
				tireTorqueMag = tire.m_torque;
			}

			tire.m_torque -= tireTorqueMag;
		} 	
	}
}
void* dNewtonBody::GetVelocity()
{
	NewtonBodyGetVelocity(m_body, &m_velocity.m_x);
	return &m_velocity;
}