C++ dAbs函数代码示例

	PlaformEntityEntity (DemoEntityManager* const scene, DemoEntity* const source, NewtonBody* const triggerPort0, NewtonBody* const triggerPort1)
		:DemoEntity (source->GetNextMatrix(), NULL)
	{
		scene->Append(this);

		DemoMesh* const mesh = (DemoMesh*)source->GetMesh();
		dAssert (mesh->IsType(DemoMesh::GetRttiType()));

		SetMesh(mesh, source->GetMeshMatrix());

		const dFloat mass = 100.0f;
		dMatrix matrix (source->GetNextMatrix()) ;
		NewtonWorld* const world = scene->GetNewton();

		// note: because the mesh matrix can have scale, for simplicity just apply the local mesh matrix to the vertex cloud
		dVector pool[128];
		const dMatrix& meshMatrix = GetMeshMatrix();
		meshMatrix.TransformTriplex(&pool[0].m_x, sizeof (dVector), mesh->m_vertex, 3 * sizeof (dFloat), mesh->m_vertexCount);
		NewtonCollision* const collision = NewtonCreateConvexHull(world, mesh->m_vertexCount, &pool[0].m_x, sizeof (dVector), 0, 0, NULL);

		NewtonBody* body = CreateSimpleBody (world, this, 100, matrix, collision, 0);
		NewtonDestroyCollision(collision);


		// attach a kinematic joint controller joint to move this body 
		dVector pivot;
		NewtonBodyGetCentreOfMass (body, &pivot[0]);
		pivot = matrix.TransformVector(pivot);
		m_driver = new FerryDriver (body, pivot, triggerPort0, triggerPort1);
		m_driver->SetMaxLinearFriction (50.0f * dAbs (mass * DEMO_GRAVITY)); 
		m_driver->SetMaxAngularFriction(50.0f * dAbs (mass * DEMO_GRAVITY)); 
	}

CustomVehicleControllerComponentEngine::dGearBox::dGearState* CustomVehicleControllerComponentEngine::dGearBox::dGearState::Update(CustomVehicleController* const vehicle)
{
    const CustomVehicleControllerComponentEngine* const engine = vehicle->GetEngine();

    dFloat param = engine->GetParam();
    dFloat speed = engine->GetSpeed();
    dFloat normalrpm = engine->GetRPM() / engine->GetRedLineRPM();

    if ((normalrpm > m_shiftUp) && (speed > 1.0f)) {
        return m_next;
    } else if (normalrpm < m_shiftDown) {
        if ((dAbs (speed) < 0.5f) && (param < dFloat (1.0e-3f))) {
            return m_neutral;
        }

        if ((dAbs (speed) < 2.0f) && (param < dFloat (-1.0e-3f))) {
            return m_reverse;
        }

        if (param > dFloat (-1.0e-3f)) 

        dAssert (m_prev != m_neutral);
        dAssert (m_prev != m_reverse);
        return m_prev;
    } else if (param < 0.0f) {
        dAssert (0);
/*
        if (speed < 1.0f) {
            return m_reverse;
        }
*/
    }

    return this;
}

CustomVehicleControllerComponentTrackSkidSteering::CustomVehicleControllerComponentTrackSkidSteering (CustomVehicleController* const controller, dFloat steeringRPM, dFloat teeringTorque)
	:CustomVehicleControllerComponentSteering (controller, 0.0f)
	,m_steeringRPM (dAbs(steeringRPM))
	,m_steeringTorque (dAbs(teeringTorque))
{
	m_differencialTurnRate = m_steeringTorque / (m_steeringRPM * m_steeringRPM);
}

void Custom6DOF::SetAngularLimits (const dVector& minAngularLimits, const dVector& maxAngularLimits)
{
	for (int i = 0; i < 3; i ++) {
		m_minAngularLimits[i] =  (dAbs (minAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : minAngularLimits[i];
		m_maxAngularLimits[i] =  (dAbs (maxAngularLimits[i]) < dFloat (1.0e-5f)) ? 0.0f : maxAngularLimits[i];
	}
}

void dNewtonCollision::SetScale(dFloat scaleX, dFloat scaleY, dFloat scaleZ)
{
	scaleX = dMax(0.01f, dAbs(scaleX));
	scaleY = dMax(0.01f, dAbs(scaleY));
	scaleZ = dMax(0.01f, dAbs(scaleZ));
	NewtonCollisionSetScale(m_shape, scaleX, scaleY, scaleZ);
}

dQuaternion::dQuaternion (const dMatrix &matrix)
{
	enum QUAT_INDEX
	{
		X_INDEX=0,
		Y_INDEX=1,
		Z_INDEX=2
	};
	static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};

	dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
	dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);

	if (trace > dFloat(0.0f)) {
		trace = dSqrt (trace + dFloat(1.0f));
		m_q0 = dFloat (0.5f) * trace;
		trace = dFloat (0.5f) / trace;
		m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
		m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
		m_q3 = (matrix[0][1] - matrix[1][0]) * trace;

	} else {
		QUAT_INDEX i = X_INDEX;
		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
			i = Y_INDEX;
		}
		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
			i = Z_INDEX;
		}
		QUAT_INDEX j = QIndex [i];
		QUAT_INDEX k = QIndex [j];

		trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
		trace = dSqrt (trace);

		dFloat* const ptr = &m_q1;
		ptr[i] = dFloat (0.5f) * trace;
		trace = dFloat (0.5f) / trace;
		m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
	}

#if _DEBUG

	dMatrix tmp (*this, matrix.m_posit);
	dMatrix unitMatrix (tmp * matrix.Inverse());
	for (int i = 0; i < 4; i ++) {
		dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
		dAssert (err < dFloat (1.0e-3f));
	}

	dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
	dAssert (err < dFloat(1.0e-3f));
#endif

}

bool dMatrix::TestIdentity() const
{
   const dMatrix & matrix = *this;
   const dMatrix & identity = dGetIdentityMatrix();
   bool isIdentity = true;
   for (int i = 0; isIdentity && (i < 3); i ++)
   {
      isIdentity &= dAbs (matrix[3][i]) < 1.0e-4f;
      for (int j = i; isIdentity && (j < 3); j ++)
         isIdentity &= dAbs (matrix[i][j] - identity[i][j]) < 1.0e-4f;
   }
   return isIdentity;
}

dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist)
{
	// shot a vertical ray from a high altitude and collect the intersection parameter.
	dVector p0 (origin); 
	dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f)); 

	dFloat parameter = 1.2f;
	NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, &parameter, RayPrefilter, 0);
	if (parameter < 1.0f) {
		p0 -= dVector (0.0f, dAbs (dist) * parameter, 0.0f, 0.0f);
	}
	return p0;
}

dFloat CustomDGRayCastCar::CalculateLongitudinalForce (int tireIndex, dFloat hubSpeed, dFloat tireLoad) const
{
	dFloat force = 0.0f;
	Tire& tire = m_tires[tireIndex];

//hubSpeed = 10.0;
//tire.m_torque = 500;

	dFloat velocAbs = dAbs (hubSpeed);
	if (velocAbs > 1.0e-2f) {
		dFloat den = 1.0f / velocAbs;

		float omega0 = hubSpeed;

		float maxSlip = m_normalizedLongitudinalForce.GetMaxValue ();
		float omega1 = velocAbs * maxSlip + hubSpeed;
		if (tire.m_torque < 0.0f) {
			//maxSlip *= -1.0f;
			omega1 = velocAbs * maxSlip - hubSpeed;
		}
//		float omega1 = velocAbs * maxSlip + hubSpeed;
		//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
	
		dFloat omega = 0.0f;
		dFloat slipRatio = 0.0f;
		for (int i = 0; i < 32; i ++) {
			omega  = (omega0 + omega1) * 0.5f;
			slipRatio = den * (omega - hubSpeed);
			force = m_normalizedLongitudinalForce.GetValue (slipRatio) * tireLoad;
			dFloat torque = tire.m_torque - tire.m_radius * force;
			if (torque > 1.0e-1f) {
				omega0 = omega;
			} else if (torque < -1.0e-1f) {
				omega1 = omega;
			} else {
				break;
			}
		}
		tire.m_angularVelocity = -omega / tire.m_radius;


	} else {
		if (dAbs (tire.m_torque) > 0.1f) {
			_ASSERTE (0);
		} 

		//dFloat slipRatioCoef = (dAbs (axelLinearSpeed) > 1.e-3f) ? ((tireRotationSpeed - axelLinearSpeed) / dAbs (axelLinearSpeed)) : 0.0f;
	}

	return force;
}

bool dMatrix::SanityCheck() const
{
   dVector right (m_front * m_up);
   if (dAbs (right % m_right) < 0.9999f)
      return false;
   if (dAbs (m_right.m_w) > 0.0f)
      return false;
   if (dAbs (m_up.m_w) > 0.0f)
      return false;
   if (dAbs (m_right.m_w) > 0.0f)
      return false;
   if (dAbs (m_posit.m_w) != 1.0f)
      return false;
   return true;
}

dQuaternion::dQuaternion (const dVector &unitAxis, dFloat angle)
{
	angle *= dFloat (0.5f);
	m_q0 = dCos (angle);
	dFloat sinAng = dSin (angle);

#ifdef _DEBUG
	if (dAbs (angle) > dFloat(1.0e-6f)) {
		dAssert (dAbs (dFloat(1.0f) - unitAxis % unitAxis) < dFloat(1.0e-3f));
	} 
#endif
	m_q1 = unitAxis.m_x * sinAng;
	m_q2 = unitAxis.m_y * sinAng;
	m_q3 = unitAxis.m_z * sinAng;
}

dMatrix dMatrix::Inverse4x4 () const
{
   const dFloat tol = 1.0e-4f;
   dMatrix tmp (*this);
   dMatrix inv (dGetIdentityMatrix());
   for (int i = 0; i < 4; i ++)
   {
      dFloat diag = tmp[i][i];
      if (dAbs (diag) < tol)
      {
         int j = 0;
         for (j = i + 1; j < 4; j ++)
         {
            dFloat val = tmp[j][i];
            if (dAbs (val) > tol)
               break;
         }
         dAssert (j < 4);
         for (int k = 0; k < 4; k ++)
         {
            tmp[i][k] += tmp[j][k];
            inv[i][k] += inv[j][k];
         }
         diag = tmp[i][i];
      }
      dFloat invDiag = 1.0f / diag;
      for (int j = 0; j < 4; j ++)
      {
         tmp[i][j] *= invDiag;
         inv[i][j] *= invDiag;
      }
      tmp[i][i] = 1.0f;
      for (int j = 0; j < 4; j ++)
      {
         if (j != i)
         {
            dFloat pivot = tmp[j][i];
            for (int k = 0; k < 4; k ++)
            {
               tmp[j][k] -= pivot * tmp[i][k];
               inv[j][k] -= pivot * inv[i][k];
            }
            tmp[j][i] = 0.0f;
         }
      }
   }
   return inv;
}

void dCustomTireSpringDG::SubmitConstraints(dFloat timestep, int threadIndex)
{
	NewtonBody* BodyAttach;
	//NewtonBody* BodyFrame;
	//
	dVector tireOmega = dVector(0.0f, 0.0f, 0.0f);
	//BodyFrame = GetBody0();
	BodyAttach = GetBody1();
	//
	SteeringController(timestep);
	//
	// calculate the position of the pivot point and the Jacobian direction vectors, in global space. 
	CalculateGlobalMatrix(mChassisPivotMatrix, mTirePivotMatrix);
	//
	NewtonBodyGetOmega(BodyAttach, &tireOmega[0]);
	//
    mRealOmega = dAbs(tireOmega.DotProduct3(mChassisPivotMatrix.m_front));
	//
	TireCenterPin(timestep);
	//
	TireCenterBolt(timestep);
    //
	SuspenssionSpringLimits(timestep);
	//
	TireBreakAction(BodyAttach, timestep);
}

void CustomVehicleControllerComponentBrake::Update (dFloat timestep)
{
	for (dList<dList<CustomVehicleControllerBodyStateTire>::dListNode*>::dListNode* node = m_brakeTires.GetFirst(); node; node = node->GetNext()) {
		CustomVehicleControllerBodyStateTire& tire = node->GetInfo()->GetInfo();
		tire.m_brakeTorque = dMax (tire.m_brakeTorque, dAbs (m_maxBrakeTorque * m_param));
	}
}

dVector FindFloor (const NewtonWorld* world, const dVector& origin, dFloat dist, dVector* const normal)
{
	// shot a vertical ray from a high altitude and collect the intersection parameter.
	dVector p0 (origin); 
	dVector p1 (origin - dVector (0.0f, dAbs (dist), 0.0f, 0.0f)); 

	RayCastPlacementData parameter;
	NewtonWorldRayCast (world, &p0[0], &p1[0], RayCastPlacement, &parameter, RayPrefilter, 0);
	if (parameter.m_param < 1.0f) {
		p0 -= dVector (0.0f, dAbs (dist) * parameter.m_param, 0.0f, 0.0f);
		if (normal) {
			*normal = parameter.m_normal;
		}
	}
	return p0;
}