Пример #1
0
dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const
{
    dFloat dot;
    dFloat ang;
    dFloat Sclp;
    dFloat Sclq;
    dFloat den;
    dFloat sinAng;
    dQuaternion Q;

    dot = DotProduct (QB);
    _ASSERTE (dot >= 0.0f);

    if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
        if (dot < dFloat(0.995f)) {

            ang = dAcos (dot);

            sinAng = dSin (ang);
            den = dFloat(1.0f) / sinAng;

            Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
            Sclq = dSin (t * ang) * den;

        } else  {
            Sclp = dFloat(1.0f) - t;
            Sclq = t;
        }

        Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq;
        Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq;
        Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq;
        Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq;

    } else {
        Q.m_q0 =  m_q3;
        Q.m_q1 = -m_q2;
        Q.m_q2 =  m_q1;
        Q.m_q3 =  m_q0;

        Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f));
        Sclq = dSin (t * dFloat (3.141592f * 0.5f));

        Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq;
        Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq;
        Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq;
        Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq;
    }

    dot = Q.DotProduct (Q);
    if ((dot) < (1.0f - 1.0e-4f) ) {
        dot = dFloat(1.0f) / dSqrt (dot);
        //dot = dgRsqrt (dot);
        Q.m_q0 *= dot;
        Q.m_q1 *= dot;
        Q.m_q2 *= dot;
        Q.m_q3 *= dot;
    }
    return Q;
}
void DynamicRagDollJoint::SetConeAngle(dFloat angle)
{
	m_coneAngle = angle;
	m_coneAngleCos = dCos(angle);
	m_coneAngleSin = dSin(angle);
	m_coneAngleHalfCos = dCos(angle * 0.5f);
	m_coneAngleHalfSin = dSin(angle * 0.5f);
}
void CustomLimitBallAndSocket::SetConeAngle (dFloat angle)
{
	m_coneAngle = angle;
	m_coneAngleCos = dCos (angle);
	m_coneAngleSin = dSin (angle);
	m_coneAngleHalfCos = dCos (angle * 0.5f);
	m_coneAngleHalfSin = dSin (angle * 0.5f);
}
Пример #4
0
	// this calculate the desired position and orientation in the path
	dMatrix BuildMatrix (dFloat timestep)
	{
		dMatrix matrix (dPitchMatrix(-m_angle) * dRollMatrix(-m_angle));
		matrix.m_posit = m_origin;
		matrix.m_posit.m_z += radius * dSin (m_angle);
		matrix.m_posit.m_y += radius * dCos (m_angle);
		return matrix;
	}
Пример #5
0
static inline dReal sinc (dReal x)
{
    // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
    // is actually accurate to one LS bit within this range if double precision
    // is being used - so don't worry!
    if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
    else return dSin(x)/x;
}
Пример #6
0
	void AddCollisionHeightFieldMesh(DemoEntityManager* const scene)
	{
		int size = 101;
		dFloat* const elevation = new dFloat[size * size];

		//y = a * sin(x)* (sin (z)) ^2
		for (int i = 0; i < size; i ++) {
			dFloat z = 3.1416f * i / (size - 1);
			dFloat z2 = dSin (z);
			z2 = z2 * z2 * 5.0f;
			for (int j = 0; j < size; j ++) {
				dFloat x = 3.1416f * j / (size - 1);
				dFloat y = z2 * dSin(x);
				elevation[j * size + i] = y;
			}
		}
		dFloat cellsize = 0.25f;
		DemoMesh* const mesh = new DemoMesh ("terrain", elevation, size, cellsize, 1.0f/16.0f, size - 1);
		
		int width = size;
		int height = size;
		char* const attibutes = new char [size * size];
		memset (attibutes, 0, width * height * sizeof (char));
		NewtonCollision* const collision = NewtonCreateHeightFieldCollision (scene->GetNewton(), width, height, 1, 0, elevation, attibutes, 1.0f, cellsize, 0);

		void* const proxy  = NewtonSceneCollisionAddSubCollision (m_sceneCollision, collision);
		NewtonDestroyCollision(collision);


		// set the parameter on the added collision share
		dMatrix matrix (dGetIdentityMatrix());
		matrix.m_posit.m_y = -2.0f;
		matrix.m_posit.m_x = -15.0f;
		matrix.m_posit.m_z = -10.0f;
		NewtonCollision* const terrainCollision = NewtonSceneCollisionGetCollisionFromNode (m_sceneCollision, proxy );

		NewtonSceneCollisionSetSubCollisionMatrix (m_sceneCollision, proxy, &matrix[0][0]);	
		NewtonCollisionSetUserData(terrainCollision, mesh);

		delete[] elevation;
		delete[] attibutes;
	}
Пример #7
0
dMatrix dRollMatrix (dFloat ang)
{
   dFloat cosAng;
   dFloat sinAng;
   sinAng = dSin (ang);
   cosAng = dCos (ang);
   return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f),
                   dVector (-sinAng, cosAng, 0.0f, 0.0f),
                   dVector (   0.0f,   0.0f, 1.0f, 0.0f),
                   dVector (   0.0f,   0.0f, 0.0f, 1.0f));
}
Пример #8
0
void CustomDGRayCastCar::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
	m_tires[index].m_steerAngle = angle;
	m_tires[index].m_localAxis.m_z = dCos (angle);
	m_tires[index].m_localAxis.m_x = dSin (angle);

//	if (m_tiresRollSide==0) {
//		m_tires[index].m_turnforce = turnforce;
//	} else {
//		m_tires[index].m_turnforce = -turnforce;
//	}
}
Пример #9
0
EXPORT_C void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
  dReal sphi,cphi,stheta,ctheta,spsi,cpsi;

  sphi = dSin(phi);
  cphi = dCos(phi);
  stheta = dSin(theta);
  ctheta = dCos(theta);
  spsi = dSin(psi);
  cpsi = dCos(psi);
  _R(0,0) = dMUL(cpsi,ctheta);
  _R(0,1) = dMUL(spsi,ctheta);
  _R(0,2) =-stheta;
  _R(0,3) = REAL(0.0);
  _R(1,0) = dMUL(cpsi,dMUL(stheta,sphi)) - dMUL(spsi,cphi);
  _R(1,1) = dMUL(spsi,dMUL(stheta,sphi)) + dMUL(cpsi,cphi);
  _R(1,2) = dMUL(ctheta,sphi);
  _R(1,3) = REAL(0.0);
  _R(2,0) = dMUL(cpsi,dMUL(stheta,cphi)) + dMUL(spsi,sphi);
  _R(2,1) = dMUL(spsi,dMUL(stheta,cphi)) - dMUL(cpsi,sphi);
  _R(2,2) = dMUL(ctheta,cphi);
  _R(2,3) = REAL(0.0);
}
Пример #10
0
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
{
  dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
  dAASSERT (R);
  sphi = dSin(phi);
  cphi = dCos(phi);
  stheta = dSin(theta);
  ctheta = dCos(theta);
  spsi = dSin(psi);
  cpsi = dCos(psi);
  _R(0,0) = cpsi*ctheta;
  _R(0,1) = spsi*ctheta;
  _R(0,2) =-stheta;
  _R(0,3) = REAL(0.0);
  _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
  _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
  _R(1,2) = ctheta*sphi;
  _R(1,3) = REAL(0.0);
  _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
  _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
  _R(2,2) = ctheta*cphi;
  _R(2,3) = REAL(0.0);
}
Пример #11
0
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;
}
void _InitCylinderTrimeshData(sData& cData)
{
    // get cylinder information
    // Rotation
    const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
    dMatrix3Copy(pRotCyc,cData.mCylinderRot);
    dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);

    // Position
    const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
    dVector3Copy(*pPosCyc,cData.vCylinderPos);
    // Cylinder axis
    dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
    // get cylinder radius and size
    dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);

    // get trimesh position and orientation
    const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
    dMatrix3Copy(pRotTris,cData.mTrimeshRot);
    dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);

    // Position
    const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
    dVector3Copy(*pPosTris,cData.vTrimeshPos);


    // calculate basic angle for 8-gon
    dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
    // calculate angle increment
    dReal fAngleIncrement = fAngle*REAL(2.0);

    // calculate plane normals
    // axis dependant code
    for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
    {
        cData.avCylinderNormals[i][0] = -dCos(fAngle);
        cData.avCylinderNormals[i][1] = -dSin(fAngle);
        cData.avCylinderNormals[i][2] = REAL(0.0);

        fAngle += fAngleIncrement;
    }

    dSetZero(cData.vBestPoint,4);
    // reset best depth
    cData.fBestCenter = REAL(0.0);
}
Пример #13
0
	dEffectorWalkPoseGenerator(NewtonBody* const rootBody)
		:dEffectorTreeFixPose(rootBody)
		,m_acc(0.0f)
		,m_amplitud_x(0.35f)
		,m_amplitud_y(0.1f)
		,m_period (1.0f)
		,m_cycle()
	{
		m_sequence[0] = 0;
		m_sequence[3] = 0;
		m_sequence[4] = 0;
		m_sequence[1] = 1;
		m_sequence[2] = 1;
		m_sequence[5] = 1;

		// make left walk cycle
		const int size = 11;
		const int splite = (size - 1) / 2 - 1;
		dFloat64 knots[size];
		dBigVector leftControlPoints[size + 2];
		for (int i = 0; i < size; i ++) {
			knots[i] = dFloat (i) / (size - 1);
		}
		memset (leftControlPoints, 0, sizeof (leftControlPoints));

		dFloat x = -m_amplitud_x / 2.0f;
		dFloat step_x = m_amplitud_x / splite;
		for (int i = 0; i <= splite; i ++) {
			leftControlPoints[i + 1].m_y = m_amplitud_y * dSin (dPi * dFloat (i) / splite);
			leftControlPoints[i + 1].m_x = x;
			x += step_x;
		}

		x = m_amplitud_x / 2.0f;
		step_x = -m_amplitud_x / (size - splite - 1);
		for (int i = splite; i < size; i++) {
			leftControlPoints[i + 1].m_x = x;
			x += step_x;
		}
		leftControlPoints[0].m_x = leftControlPoints[1].m_x;
		leftControlPoints[size + 1].m_x = leftControlPoints[size].m_x;

//		cycle.CreateFromKnotVectorAndControlPoints(3, size, knots, leftControlPoints);
		m_cycle.CreateFromKnotVectorAndControlPoints(1, size, knots, &leftControlPoints[1]);
	}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	dMatrix localMatrix(matrix0 * matrix1.Inverse());
	dVector euler0;
	dVector euler1;
	localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);

	dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f));
	rollPin = matrix1.RotateVector(rollPin);

	NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	m_curJointAngle.Update(euler0.m_x);

	// save the current joint Omega
	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front);

	if (m_options.m_option2) {
		if (m_options.m_option3) {
			dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep);
		} else {
			dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep);
		}
	} else if (m_options.m_option3) {
		dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep);
	} else if (m_angularFriction != 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep);
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
	}
}
Пример #15
0
EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
			 dReal angle)
{
  dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az);
  if (l > REAL(0.0)) {
    angle = dMUL(angle,REAL(0.5));
    q[0] = dCos (angle);
    l = dMUL(dReal(dSin(angle)),dRecipSqrt(l));
    q[1] = dMUL(ax,l);
    q[2] = dMUL(ay,l);
    q[3] = dMUL(az,l);
  }
  else {
    q[0] = REAL(1.0);
    q[1] = 0;
    q[2] = 0;
    q[3] = 0;
  }
}
Пример #16
0
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
       dReal angle)
{
  dAASSERT (q);
  dReal l = ax*ax + ay*ay + az*az;
  if (l > REAL(0.0)) {
    angle *= REAL(0.5);
    q[0] = dCos (angle);
    l = dSin(angle) * dRecipSqrt(l);
    q[1] = ax*l;
    q[2] = ay*l;
    q[3] = az*l;
  }
  else {
    q[0] = 1;
    q[1] = 0;
    q[2] = 0;
    q[3] = 0;
  }
}
dFloat AngularIntegration::update(dFloat angle) {
    return update(dCos(angle), dSin(angle));
}
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const
{
	dMatrix matrix0;
	dMatrix matrix1;

	dCustomJoint::Debug(debugDisplay);

	CalculateGlobalMatrix(matrix0, matrix1);

	const dVector& coneDir0 = matrix0.m_front;
	const dVector& coneDir1 = matrix1.m_front;
	dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1);
	dMatrix coneRotation(dGetIdentityMatrix());
	if (cosAngleCos < 0.9999f) {
		dVector lateralDir(coneDir1.CrossProduct(coneDir0));
		dFloat mag2 = lateralDir.DotProduct3(lateralDir);
		//dAssert(mag2 > 1.0e-4f);
		if (mag2 > 1.0e-4f) {
			lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
			coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit);
		} else {
			lateralDir = matrix0.m_up.Scale(-1.0f);
			coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit);
		}
	} else if (cosAngleCos < -0.9999f) {
		coneRotation[0][0] = -1.0f;
		coneRotation[1][1] = -1.0f;
	}

	const int subdiv = 18;
	const dFloat radius = debugDisplay->m_debugScale;
	dVector arch[subdiv + 1];

	// show twist angle limits
	if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) {
		dMatrix pitchMatrix(matrix1 * coneRotation);
		pitchMatrix.m_posit = matrix1.m_posit;

		dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f));

		dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv;
		dFloat angle0 = m_minTwistAngle;

		debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f));
		for (int i = 0; i <= subdiv; i++) {
			arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point));
			debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]);
			angle0 += angleStep;
		}

		for (int i = 0; i < subdiv; i++) {
			debugDisplay->DrawLine(arch[i], arch[i + 1]);
		}
	}

	// show cone angle limits
	if (m_options.m_option2) {
		dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f);
		dFloat angleStep = dPi * 2.0f / subdiv;
		dFloat angle0 = 0.0f;
		debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f));

		for (int i = 0; i <= subdiv; i++) {
			dVector conePoint(dPitchMatrix(angle0).RotateVector(point));
			dVector p(matrix1.TransformVector(conePoint));
			arch[i] = p;
			debugDisplay->DrawLine(matrix1.m_posit, p);
			angle0 += angleStep;
		}

		for (int i = 0; i < subdiv; i++) {
			debugDisplay->DrawLine(arch[i], arch[i + 1]);
		}
	}
}
Пример #19
0
void CustomDGRayCastCar::AddSingleSuspensionTire (
	void *userData,
	const dVector& localPosition, 
	dFloat mass,
	dFloat radius, 
	dFloat width,
	dFloat friction,
	dFloat suspensionLenght,
	dFloat springConst,
	dFloat springDamper,
	int castMode)
{
	dVector relTirePos = localPosition;
	suspensionLenght = dAbs ( suspensionLenght );

	dMatrix chassisMatrix;
	NewtonBodyGetMatrix (m_body0, &chassisMatrix[0][0]);
	chassisMatrix = chassisMatrix * chassisMatrix;
	relTirePos += chassisMatrix.m_up.Scale ( suspensionLenght );

	m_tires[m_tiresCount].m_harpoint              = m_localFrame.UntransformVector( relTirePos ); 
	m_tires[m_tiresCount].m_localAxis             = dVector (0.0f, 0.0f, 1.0f, 0.0f);
	m_tires[m_tiresCount].m_tireAxelPosit         = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_tireAxelVeloc         = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_lateralPin            = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_longitudinalPin       = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_hitBodyPointVelocity  = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_HitBody               = NULL;
	m_tires[m_tiresCount].m_userData              = userData;
	m_tires[m_tiresCount].m_spinAngle             = 0.0f;
	m_tires[m_tiresCount].m_steerAngle            = 0.0f;	
	m_tires[m_tiresCount].m_tireLoad              = 0.0f;

	m_tires[m_tiresCount].m_posit                 = suspensionLenght;
	m_tires[m_tiresCount].m_tireIsOnAir           = 1;
	m_tires[m_tiresCount].m_tireUseConvexCastMode = castMode; 
	
	m_tires[m_tiresCount].m_springConst           = springConst;
	m_tires[m_tiresCount].m_springDamper          = springDamper;
	m_tires[m_tiresCount].m_suspensionLenght	  = suspensionLenght;	
	m_tires[m_tiresCount].m_angularVelocity       = 0.0f;
	m_tires[m_tiresCount].m_breakForce            = 0.0f;
	m_tires[m_tiresCount].m_torque                = 0.0f;
	m_tires[m_tiresCount].m_groundFriction        = friction;
	m_tires[m_tiresCount].m_tireIsConstrained	  = 0;	

	m_tires[m_tiresCount].m_mass				  = mass;
	m_tires[m_tiresCount].m_width				  = width;
	m_tires[m_tiresCount].m_radius				  = radius;
	m_tires[m_tiresCount].m_Ixx					  = mass * radius * radius / 2.0f;
	m_tires[m_tiresCount].m_IxxInv				  = 1.0f / m_tires[m_tiresCount].m_Ixx;

#define TIRE_SHAPE_SIZE 12
	dVector shapePoints[TIRE_SHAPE_SIZE * 2];
	for ( int i = 0; i < TIRE_SHAPE_SIZE; i ++ ) {
		shapePoints[i].m_x = -width * 0.5f;	
		shapePoints[i].m_y = radius * dCos ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
		shapePoints[i].m_z = radius * dSin ( 2.0f * 3.14159265f * dFloat( i )/ dFloat( TIRE_SHAPE_SIZE ) );
		shapePoints[i + TIRE_SHAPE_SIZE].m_x = -shapePoints[i].m_x;
		shapePoints[i + TIRE_SHAPE_SIZE].m_y = shapePoints[i].m_y;
		shapePoints[i + TIRE_SHAPE_SIZE].m_z = shapePoints[i].m_z;
	}
	m_tires[m_tiresCount].m_shape = NewtonCreateConvexHull ( m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, 0, NULL );
// NewtonCreateChamferCylinder(m_world,radius,width,NULL); 
// NewtonCreateSphere(m_world,radius,radius,radius,&offmat[0][0]);
// NewtonCreateCone(m_world,radius,width,NULL);
// NewtonCreateCapsule(m_world,radius,width,NULL);
// NewtonCreateChamferCylinder(m_world,radius,width,NULL);
// NewtonCreateCylinder(m_world,radius*2,width*2,NULL);
// NewtonCreateBox(m_world,radius*2,radius*2,radius*2,NULL);
// NewtonCreateConvexHull (m_world, TIRE_SHAPE_SIZE * 2, &shapePoints[0].m_x, sizeof (dVector), 0.0f, NULL);

	m_tiresCount ++;
}
Пример #20
0
// initialize collision data
void _cldInitCylinderBox(sCylinderBoxData& cData) 
{
	// get cylinder position, orientation
	const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); 
	dMatrix3Copy(pRotCyc,cData.mCylinderRot);

	const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
	dVector3Copy(*pPosCyc,cData.vCylinderPos);

	dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
	
	// get cylinder radius and size
	dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);

	// get box position, orientation, size
	const dReal* pRotBox = dGeomGetRotation(cData.gBox);
	dMatrix3Copy(pRotBox,cData.mBoxRot);
	const dVector3* pPosBox  = (const dVector3*)dGeomGetPosition(cData.gBox);
	dVector3Copy(*pPosBox,cData.vBoxPos);

	dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
	cData.vBoxHalfSize[0] *= REAL(0.5);
	cData.vBoxHalfSize[1] *= REAL(0.5);
	cData.vBoxHalfSize[2] *= REAL(0.5);

	// vertex 0
	cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
	cData.avBoxVertices[0][1] =  cData.vBoxHalfSize[1];
	cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];

	// vertex 1
	cData.avBoxVertices[1][0] =  cData.vBoxHalfSize[0];
	cData.avBoxVertices[1][1] =  cData.vBoxHalfSize[1];
	cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];

	// vertex 2
	cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
	cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
	cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];

	// vertex 3
	cData.avBoxVertices[3][0] =  cData.vBoxHalfSize[0];
	cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
	cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];

	// vertex 4
	cData.avBoxVertices[4][0] =  cData.vBoxHalfSize[0];
	cData.avBoxVertices[4][1] =  cData.vBoxHalfSize[1];
	cData.avBoxVertices[4][2] =  cData.vBoxHalfSize[2];

	// vertex 5
	cData.avBoxVertices[5][0] =  cData.vBoxHalfSize[0];
	cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
	cData.avBoxVertices[5][2] =  cData.vBoxHalfSize[2];

	// vertex 6
	cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
	cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
	cData.avBoxVertices[6][2] =  cData.vBoxHalfSize[2];

	// vertex 7
	cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
	cData.avBoxVertices[7][1] =  cData.vBoxHalfSize[1];
	cData.avBoxVertices[7][2] =  cData.vBoxHalfSize[2];

	// temp index
	int i = 0;
	dVector3	vTempBoxVertices[8];
	// transform vertices in absolute space
	for(i=0; i < 8; i++) 
	{
		dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
		dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
	}

	// find relative position
	dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
	cData.fBestDepth = MAX_FLOAT;
	cData.vNormal[0] = REAL(0.0);
	cData.vNormal[1] = REAL(0.0);
	cData.vNormal[2] = REAL(0.0);

	// calculate basic angle for nCYLINDER_SEGMENT-gon
	dReal fAngle = M_PI/nCYLINDER_SEGMENT;

	// calculate angle increment
	dReal fAngleIncrement = fAngle * REAL(2.0); 

	// calculate nCYLINDER_SEGMENT-gon points
	for(i = 0; i < nCYLINDER_SEGMENT; i++) 
	{
		cData.avCylinderNormals[i][0] = -dCos(fAngle);
		cData.avCylinderNormals[i][1] = -dSin(fAngle);
		cData.avCylinderNormals[i][2] = 0;

		fAngle += fAngleIncrement;
	}

	cData.fBestrb		= 0;
	cData.fBestrc		= 0;
	cData.iBestAxis		= 0;
	cData.nContacts		= 0;

}
// initialize collision data
void sCylinderBoxData::_cldInitCylinderBox() 
{
	// get cylinder position, orientation
	const dReal* pRotCyc = dGeomGetRotation(m_gCylinder); 
	dMatrix3Copy(pRotCyc,m_mCylinderRot);

	const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder);
	dVector3Copy(*pPosCyc,m_vCylinderPos);

	dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis);
	
	// get cylinder radius and size
	dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize);

	// get box position, orientation, size
	const dReal* pRotBox = dGeomGetRotation(m_gBox);
	dMatrix3Copy(pRotBox,m_mBoxRot);
	const dVector3* pPosBox  = (const dVector3*)dGeomGetPosition(m_gBox);
	dVector3Copy(*pPosBox,m_vBoxPos);

	dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize);
	m_vBoxHalfSize[0] *= REAL(0.5);
	m_vBoxHalfSize[1] *= REAL(0.5);
	m_vBoxHalfSize[2] *= REAL(0.5);

	// vertex 0
	m_avBoxVertices[0][0] = -m_vBoxHalfSize[0];
	m_avBoxVertices[0][1] =  m_vBoxHalfSize[1];
	m_avBoxVertices[0][2] = -m_vBoxHalfSize[2];

	// vertex 1
	m_avBoxVertices[1][0] =  m_vBoxHalfSize[0];
	m_avBoxVertices[1][1] =  m_vBoxHalfSize[1];
	m_avBoxVertices[1][2] = -m_vBoxHalfSize[2];

	// vertex 2
	m_avBoxVertices[2][0] = -m_vBoxHalfSize[0];
	m_avBoxVertices[2][1] = -m_vBoxHalfSize[1];
	m_avBoxVertices[2][2] = -m_vBoxHalfSize[2];

	// vertex 3
	m_avBoxVertices[3][0] =  m_vBoxHalfSize[0];
	m_avBoxVertices[3][1] = -m_vBoxHalfSize[1];
	m_avBoxVertices[3][2] = -m_vBoxHalfSize[2];

	// vertex 4
	m_avBoxVertices[4][0] =  m_vBoxHalfSize[0];
	m_avBoxVertices[4][1] =  m_vBoxHalfSize[1];
	m_avBoxVertices[4][2] =  m_vBoxHalfSize[2];

	// vertex 5
	m_avBoxVertices[5][0] =  m_vBoxHalfSize[0];
	m_avBoxVertices[5][1] = -m_vBoxHalfSize[1];
	m_avBoxVertices[5][2] =  m_vBoxHalfSize[2];

	// vertex 6
	m_avBoxVertices[6][0] = -m_vBoxHalfSize[0];
	m_avBoxVertices[6][1] = -m_vBoxHalfSize[1];
	m_avBoxVertices[6][2] =  m_vBoxHalfSize[2];

	// vertex 7
	m_avBoxVertices[7][0] = -m_vBoxHalfSize[0];
	m_avBoxVertices[7][1] =  m_vBoxHalfSize[1];
	m_avBoxVertices[7][2] =  m_vBoxHalfSize[2];

	// temp index
	int i = 0;
	dVector3	vTempBoxVertices[8];
	// transform vertices in absolute space
	for(i=0; i < 8; i++) 
	{
		dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]);
		dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]);
	}

	// find relative position
	dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff);
	m_fBestDepth = MAX_FLOAT;
	m_vNormal[0] = REAL(0.0);
	m_vNormal[1] = REAL(0.0);
	m_vNormal[2] = REAL(0.0);

	// calculate basic angle for nCYLINDER_SEGMENT-gon
	dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT);

	// calculate angle increment
	dReal fAngleIncrement = fAngle * REAL(2.0); 

	// calculate nCYLINDER_SEGMENT-gon points
	for(i = 0; i < nCYLINDER_SEGMENT; i++) 
	{
		m_avCylinderNormals[i][0] = -dCos(fAngle);
		m_avCylinderNormals[i][1] = -dSin(fAngle);
		m_avCylinderNormals[i][2] = 0;

		fAngle += fAngleIncrement;
	}

	m_fBestrb		= 0;
	m_fBestrc		= 0;
	m_iBestAxis		= 0;
	m_nContacts		= 0;

}
void AngularIntegration::set_angle(dFloat angle) {
    m_angle = angle;
    m_sin_angle = dSin(angle);
    m_cos_angle = dCos(angle);
}
void CustomDGRayCastCar::SetTireSteerAngleForce (int index, dFloat angle, dFloat turnforce)
{
	m_tires[index].m_steerAngle = angle;
	m_tires[index].m_localAxisInJointSpace.m_z = dCos (angle);
	m_tires[index].m_localAxisInJointSpace.m_x = dSin (angle);
}