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);
}
示例#4
0
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);
}
示例#6
0
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

}
示例#7
0
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;
}
示例#8
0
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;
}
示例#10
0
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;
}
示例#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;
}
示例#12
0
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);
}
CustomVehicleControllerComponentSteering::CustomVehicleControllerComponentSteering (CustomVehicleController* const controller, dFloat maxAngleInRadians)
	:CustomVehicleControllerComponent (controller)
	,m_maxAngle (dAbs (maxAngleInRadians))
	,m_akermanWheelBaseWidth(0.0f)
	,m_akermanAxelSeparation(0.0f)
{
}
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));
	}
}
示例#16
0
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;
}
示例#17
0
文件: vhtjako.c 项目: jedbrown/dohp
static dErr JakoSIAVelocity(VHTCase scase,dReal b,dReal h,dReal dh[2],dReal z,dScalar u[3])
{
  struct VHTRheology *rheo = &scase->rheo;
  const dReal
    p = rheo->pe,
    n = 1 ? 1 : 1./(p-1),
    B = rheo->B0 * pow(2*rheo->gamma0,(n-1)/(2*n)), // reduce to Stress = B |Du|^{1/n}
    A = pow(B,-n);
  const dScalar
    slope = dSqrt(dSqr(dh[0]) + dSqr(dh[1])),
    sliding = 0,
    hmz = z > h ? 0 : (z < b ? h-b : h-z),
    // The strain rate is: Du = A tau^n
    // where: tau = rho*grav*(h-z)*dh
    int_bz = -1. / (n+1) * (pow(hmz,n+1) - pow(h-b,n+1)), // \int_b^z (h-z)^n
    //bstress = rheo->rhoi * dAbs(rheo->gravity[2]) * (h-b) * slope, // diagnostic
    //rstress = dUnitNonDimensionalizeSI(scase->utable.Pressure,1e5),
    siaspeed = sliding + A * pow(rheo->rhoi * dAbs(rheo->gravity[2]) * slope, n) * int_bz, // Integrate strain rate from the bottom
    speed = siaspeed * (5 + 0 * (1 + tanh((z-b)/100))/2);
  u[0] = -speed / slope * dh[0];
  u[1] = -speed / slope * dh[1];
  u[2] = 0; // Would need another derivative to evaluate this and it should not be a big deal for this stationary computation
  //printf("u0 %g  u1 %g  rho %g  grav %g  stress %g  1bar %g  dh[2] %g %g  A %g  B %g;\n",u[0],u[1],rheo->rhoi,rheo->gravity[2],bstress,rstress,dh[0],dh[1],A,B);
  return 0;
}
示例#18
0
bool dMatrix::TestOrthogonal() const
{
   dVector n (m_front * m_up);
   dFloat a = m_right % m_right;
   dFloat b = m_up % m_up;
   dFloat c = m_front % m_front;
   dFloat d = n % m_right;
   return (m_front[3] == dFloat (0.0f)) &
          (m_up[3] == dFloat (0.0f)) &
          (m_right[3] == dFloat (0.0f)) &
          (m_posit[3] == dFloat (1.0f)) &
          (dAbs (a - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (b - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (c - dFloat (1.0f)) < dFloat (1.0e-4f)) &
          (dAbs (d - dFloat (1.0f)) < dFloat (1.0e-4f));
}
示例#19
0
dFloat dPolygonRayCast (const dVector& l0, const dVector& l1, int indexCount, const dFloat* const vertex, int strideInBytes, const int* const indices)
{
	int stride = strideInBytes / sizeof (dFloat);

	dBigVector line0 (l0);
	dBigVector line1(l1);
	dBigVector segment (line1 - line0);
	dBigVector normal (dPolygonNormal (indexCount, vertex, strideInBytes, indices));
	double den = normal % segment;
	if (dAbs(den) < 1.0e-6) {
		return 1.2f;
	}
	
	double sign = (den < 0.0f) ? 1.0f : -1.0f;

	int index = indices[indexCount - 1] * stride;
	dBigVector v0 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
	dBigVector p0v0 (v0 - line0);
	for (int i = 0; i < indexCount; i ++) {
		index = indices[i] * stride;
		dBigVector v1 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
		dBigVector p0v1 (v1 - line0);
		double alpha = sign * ((p0v1 * p0v0) % segment);
		if (alpha < 1.0e-3f) {
			return 1.2f;
		}
		p0v0 = p0v1;
	}

	double t = - ((line0 - v0) % normal) / den;
	if ((t < 0.0f) || (t > 1.0f)) {
		return 1.2f;
	}
	return dFloat (t);
}
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep)
{
	const dFloat angleError = GetMaxAngleError();
	dFloat angle0 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_up);
	NewtonUserJointAddAngularRow(m_joint, angle0, &matrix1.m_up[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle0) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle0 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	dFloat angle1 = CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right);
	NewtonUserJointAddAngularRow(m_joint, angle1, &matrix1.m_right[0]);
	NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
	if (dAbs(angle1) > angleError) {
		const dFloat alpha = NewtonUserJointCalculateRowZeroAcceleration(m_joint) + dFloat(0.25f) * angle1 / (timestep * timestep);
		NewtonUserJointSetRowAcceleration(m_joint, alpha);
	}

	// the joint angle can be determined by getting the angle between any two non parallel vectors
	m_curJointAngle.Update(-CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front));

	// 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);
	}
}
 void SetSteerAngle (dFloat angle)
 {
     if (dAbs (angle - m_steeAngle) > 1.0e-4f) {
         m_steeAngle = angle;
         dMatrix rotation (dYawMatrix(m_steeAngle));
         m_chassisLocalMatrix = rotation * m_refChassisLocalMatrix;
     }
 }
示例#22
0
void CustomDGRayCastCar::ApplyDeceleration (Tire& tire)
{
	if ( dAbs( tire.m_torque ) < 1.0e-3f ){
		dVector cvel = m_chassisVelocity;   
		cvel = cvel.Scale( m_fixDeceleration );
		cvel.m_y = m_chassisVelocity.m_y;
		NewtonBodySetVelocity( m_body0, &cvel.m_x );
	}	
}
示例#23
0
文件: vhtjako.c 项目: jedbrown/dohp
static dErr JakoGradient2(VHTCase scase,const dReal field[],dInt i,dInt j,dReal grad[2])
{
  VHTCase_Jako *jako = scase->data;
  const dReal dx = jako->mygeo[1],dy = jako->mygeo[5];
  const dInt nx = jako->nx,ny = jako->ny;
  dReal fx0,fx1,fy0,fy1;
  dFunctionBegin;
  if (i+1 >= nx) dERROR(PETSC_COMM_SELF,PETSC_ERR_ARG_OUTOFRANGE,"X slope would evaluate point (%D,%D) outside of valid range %Dx%D",i+1,j,nx,ny);
  if (j-1 < 0) dERROR(PETSC_COMM_SELF,PETSC_ERR_ARG_OUTOFRANGE,"Y slope would evaluate point (%D,%D) outside of valid range %Dx%D",i,j-1,nx,ny);
  fx0 = field[j*nx + i + 0] - field[j*nx + i - 1];
  fx1 = field[j*nx + i + 1] - field[j*nx + i + 0];
  grad[0] = (dAbs(fx0) < dAbs(fx1) ? fx0 : fx1) / dx;
  // The raster has Y axis flipped
  fy0 = field[(j+0)*nx + i] - field[(j+1)*nx + i];
  fy1 = field[(j-1)*nx + i] - field[(j+0)*nx + i];
  grad[1] = (dAbs(fy0) < dAbs(fy1) ? fy0 : fy1) / dy;
  dFunctionReturn(0);
}
void dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
	if (m_actuatorFlag) {
		dFloat jointangle = GetActuatorAngle();
		dFloat relAngle = jointangle - m_angle;
		dFloat currentSpeed = GetJointOmega();
		dFloat step = dFloat(2.0f) * m_angularRate * timestep;
		dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
		dFloat accel = (desiredSpeed - currentSpeed) / timestep;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
        NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
        NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce);
		NewtonUserJointSetRowStiffness (m_joint, 1.0f);
	} else {
		dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
	}
}
static void AddShatterEntity (DemoEntityManager* const scene, DemoMesh* const visualMesh, NewtonCollision* const collision, const ShatterEffect& shatterEffect, dVector location)
{
	dQuaternion rotation;
	SimpleShatterEffectEntity* const entity = new SimpleShatterEffectEntity (visualMesh, shatterEffect);
	entity->SetMatrix(*scene, rotation, location);
	entity->InterpolateMatrix (*scene, 1.0f);
	scene->Append(entity);

	dVector origin;
	dVector inertia;
	NewtonConvexCollisionCalculateInertialMatrix (collision, &inertia[0], &origin[0]);	

float mass = 10.0f;
int materialId = 0;

	dFloat Ixx = mass * inertia[0];
	dFloat Iyy = mass * inertia[1];
	dFloat Izz = mass * inertia[2];

	//create the rigid body
	dMatrix matrix (GetIdentityMatrix());
	matrix.m_posit = location;

	NewtonWorld* const world = scene->GetNewton();
	NewtonBody* const rigidBody = NewtonCreateBody (world, collision, &matrix[0][0]);


	entity->m_myBody = rigidBody;
	entity->m_myweight = dAbs (mass * DEMO_GRAVITY);

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

	// set the mass matrix
	NewtonBodySetMassMatrix (rigidBody, mass, 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);
}
void CustomKinematicController::SetMaxLinearFriction(dFloat accel)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
	m_maxLinearFriction = dAbs (accel) * mass;
}
示例#27
0
void CustomPickBody::SetMaxLinearFriction(dFloat accel)
{
	dFloat Ixx;
	dFloat Iyy;
	dFloat Izz;
	dFloat mass;

	NewtonBodyGetMassMatrix (m_body0, &mass, &Ixx, &Iyy, &Izz);
	m_maxLinearFriction = dAbs (accel) * mass;
}
示例#28
0
int dPackVertexArray (dFloat* const vertexList, int compareElements, int strideInBytes, int vertexCount, int* const indexListOut)
{
	int stride = strideInBytes / sizeof (dFloat);
	dFloat errorTol = dFloat (1.0e-4f);

	dFloat* const array = new dFloat[(stride + 2) * vertexCount];
	for (int i = 0; i < vertexCount; i ++) {
		memcpy (&array[i * (stride + 2)], &vertexList[i * stride], strideInBytes);
		array[i * (stride + 2) + stride + 0] = dFloat(i);
		array[i * (stride + 2) + stride + 1] = 0.0f;
	}

	qsort(array, vertexCount, (stride + 2) * sizeof (dFloat), SortVertexArray);
	int indexCount = 0;
	for (int i = 0; i < vertexCount; i ++) {
		int index = i * (stride + 2);
		if (array[index + stride + 1] == 0.0f) {
			dFloat window = array[index] + errorTol; 
			for (int j = i + 1; j < vertexCount; j ++) {
				int index2 = j * (stride + 2);
				if (array[index2] >= window) {
					break;
				}
				if (array[index2 + stride + 1] == 0.0f) {
					int k;
					for (k = 0; k < compareElements; k ++) {
						dFloat error;
						error = array[index + k] - array[index2+ k];
						if (dAbs (error) >= errorTol) {
							break;
						}
					}
					if (k >= compareElements) {
						int m = int (array[index2 + stride]);
						memcpy (&array[indexCount * (stride + 2)], &array[index2], sizeof (dFloat) * stride);
						indexListOut[m] = indexCount;
						array[index2 + stride + 1] = 1.0f;
					}
				}
			}
			int m = int (array[index + stride]);
			memcpy (&array[indexCount * (stride + 2)], &array[index], sizeof (dFloat) * stride);
			indexListOut[m] = indexCount;
			array[indexCount * (stride + 2) + stride + 1] = 1.0f;
			indexCount ++;
		}
	}

	for (int i = 0; i < indexCount; i ++) {
		memcpy (&vertexList[i * stride], &array[i * (stride + 2)], sizeof (dFloat) * stride);
	}

	delete[] array;
	return indexCount;
}
void CustomUniversalActuator::SubmitConstraints (dFloat timestep, int threadIndex)
{
	CustomUniversal::SubmitConstraints (timestep, threadIndex);

	if (m_flag0 | m_flag1){
		dMatrix matrix0;
		dMatrix matrix1;

		CalculateGlobalMatrix (matrix0, matrix1);
		if (m_flag0) {
			dFloat jointAngle = GetJointAngle_0();
			dFloat relAngle = jointAngle - m_angle0;
			NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix0.m_front[0]);
			dFloat step = m_angularRate0 * timestep;
			if (dAbs (relAngle) > 2.0f * dAbs (step)) {
				dFloat desiredSpeed = dSign(relAngle) * m_angularRate0;
				dFloat currentSpeed = GetJointOmega_0 ();
				dFloat accel = (desiredSpeed - currentSpeed) / timestep;
				NewtonUserJointSetRowAcceleration (m_joint, accel);
			}
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce0);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce0);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		}

		if (m_flag1) {
			dFloat jointAngle = GetJointAngle_1();
			dFloat relAngle = jointAngle - m_angle1;
			NewtonUserJointAddAngularRow (m_joint, -relAngle, &matrix1.m_up[0]);
			dFloat step = m_angularRate1 * timestep;
			if (dAbs (relAngle) > 2.0f * dAbs (step)) {
				dFloat desiredSpeed = dSign(relAngle) * m_angularRate1;
				dFloat currentSpeed = GetJointOmega_1 ();
				dFloat accel = (desiredSpeed - currentSpeed) / timestep;
				NewtonUserJointSetRowAcceleration (m_joint, accel);
			}
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce1);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce1);
			NewtonUserJointSetRowStiffness (m_joint, 1.0f);
		}
	}
}
示例#30
0
//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
void dMatrix::PolarDecomposition (dMatrix & transformMatrix, dVector & scale, dMatrix & stretchAxis, const dMatrix & initialStretchAxis) const
{
   // a polar decomposition decompose matrix A = O * S
   // where S = sqrt (transpose (L) * L)
   // calculate transpose (L) * L
   dMatrix LL ((*this) * Transpose());
   // check is this si a pure uniformScale * rotation * translation
   dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
   dFloat invdet2 = 1.0f / det2;
   dMatrix pureRotation (LL);
   pureRotation[0] = pureRotation[0].Scale (invdet2);
   pureRotation[1] = pureRotation[1].Scale (invdet2);
   pureRotation[2] = pureRotation[2].Scale (invdet2);
   //dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
   dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
   dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
   if (dAbs (det - 1.0f) < 1.e-5f)
   {
      // this is a pure scale * rotation * translation
      det = sign * dSqrt (det2);
      scale[0] = det;
      scale[1] = det;
      scale[2] = det;
      scale[3] = 1.0f;
      det = 1.0f / det;
      transformMatrix.m_front = m_front.Scale (det);
      transformMatrix.m_up = m_up.Scale (det);
      transformMatrix.m_right = m_right.Scale (det);
      transformMatrix[0][3] = 0.0f;
      transformMatrix[1][3] = 0.0f;
      transformMatrix[2][3] = 0.0f;
      transformMatrix.m_posit = m_posit;
      stretchAxis = dGetIdentityMatrix();
   }
   else
   {
      stretchAxis = LL.JacobiDiagonalization (scale, initialStretchAxis);
      // I need to deal with buy seeing of some of the Scale are duplicated
      // do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
      // for now just us the matrix
      scale[0] = sign * dSqrt (scale[0]);
      scale[1] = sign * dSqrt (scale[1]);
      scale[2] = sign * dSqrt (scale[2]);
      scale[3] = 1.0f;
      dMatrix scaledAxis;
      scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
      scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
      scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
      scaledAxis[3] = stretchAxis[3];
      dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
      transformMatrix = symetricInv * (*this);
      transformMatrix.m_posit = m_posit;
   }
}