Esempio n. 1
0
void Import::LoadGeometries (dScene& scene, GeometryCache& meshCache, const MaterialCache& materialCache)
{
    dScene::Iterator iter (scene);
    for (iter.Begin(); iter; iter ++) {
        dScene::dTreeNode* const geometryNode = iter.GetNode();
        dNodeInfo* const info = scene.GetInfoFromNode(geometryNode);
        if (info->IsType(dGeometryNodeInfo::GetRttiType())) {
            if (info->GetTypeId() == dMeshNodeInfo::GetRttiType()) {

                // add the vertices
                //TriObject* const geometry = CreateNewTriObject();
                TriObject* const geometry = (TriObject*) CreateInstance (GEOMOBJECT_CLASS_ID, Class_ID(TRIOBJ_CLASS_ID, 0));
                meshCache.AddMesh(geometry, geometryNode);

                dMeshNodeInfo* const meshInfo = (dMeshNodeInfo*) scene.GetInfoFromNode(geometryNode);
                NewtonMesh* const mesh = meshInfo->GetMesh();

                NewtonMeshTriangulate (mesh);

                int vertexCount = NewtonMeshGetVertexCount(mesh);
                int pointCount = NewtonMeshGetPointCount(mesh);

                //int triangleCount = NewtonMeshGetTotalFaceCount(mesh);
                int triangleCount = 0;
                for (void* face = NewtonMeshGetFirstFace(mesh); face; face = NewtonMeshGetNextFace(mesh, face)) {
                    if (!NewtonMeshIsFaceOpen(mesh, face)) {
                        triangleCount += NewtonMeshGetFaceIndexCount (mesh, face) - 2;
                    }
                }

                Mesh&  maxMesh = geometry->mesh;
                maxMesh.setNumVerts(vertexCount);
                maxMesh.setNumFaces(triangleCount);
                maxMesh.setNumTVerts(pointCount);
                maxMesh.setNumTVFaces(triangleCount);

                int vertexStride = NewtonMeshGetVertexStrideInByte(mesh) / sizeof (dFloat64);
                dFloat64* const vertex = NewtonMeshGetVertexArray (mesh);
                for (int j = 0; j < vertexCount; j ++) {
                    Point3 vx (vertex[vertexStride * j + 0], vertex[vertexStride * j + 1], vertex[vertexStride * j + 2]);
                    maxMesh.setVert(j, vx);
                }

                int pointStride = NewtonMeshGetPointStrideInByte(mesh) / sizeof (dFloat64);
                dFloat64* const points = NewtonMeshGetUV0Array(mesh);
                for (int j = 0; j < pointCount; j ++) {
                    Point3 uv (dFloat(points[j * pointStride + 0]), dFloat(points[j * pointStride + 1]), 0.0f);
                    maxMesh.setTVert(j, uv);
                }

                int faceIndex = 0;
                for (void* face = NewtonMeshGetFirstFace(mesh); face; face = NewtonMeshGetNextFace(mesh, face)) {
                    if (!NewtonMeshIsFaceOpen(mesh, face)) {
                        int vertexInices[256];
                        int pointIndices[256];

                        int indexCount = NewtonMeshGetFaceIndexCount (mesh, face);
                        int matId = NewtonMeshGetFaceMaterial (mesh, face);

                        MaterialProxi material;
                        material.m_mtl = 0;
                        material.m_matID = 0;
                        MaterialCache::dTreeNode* const materialNode = materialCache.Find(matId);
                        if (materialNode) {
                            material = materialNode->GetInfo();
                        }

                        NewtonMeshGetFaceIndices (mesh, face, vertexInices);
                        NewtonMeshGetFacePointIndices (mesh, face, pointIndices);

                        for (int i = 2; i < indexCount; i ++) {
                            Face* f = &maxMesh.faces[faceIndex];
                            TVFace* t = &maxMesh.tvFace[faceIndex];

                            f->v[0] = vertexInices[0];
                            f->v[1] = vertexInices[i - 1];
                            f->v[2] = vertexInices[i];

                            f->setEdgeVis(0, 1);
                            f->setEdgeVis(1, 1);
                            f->setEdgeVis(2, 1);
                            f->setSmGroup(0);

                            //f->setMatID((MtlID)matID);
                            f->setMatID(material.m_matID);

                            t->t[0] = pointIndices[0];
                            t->t[1] = pointIndices[i - 1];
                            t->t[2] = pointIndices[i];
                            faceIndex ++;
                        }
                    }
                }

                SetSmoothingGroups (maxMesh);

#if 0
                if (geom->m_uv1) {
                    int texChannel = 2;

                    //			maxMesh.setNumMaps (texChannel, TRUE);
                    maxMesh.setMapSupport (texChannel);
                    if (maxMesh.mapSupport(texChannel)) {
                        maxMesh.setNumMapVerts (texChannel, triangleCount * 3);
                        maxMesh.setNumMapFaces (texChannel, triangleCount);

                        UVVert *tv = maxMesh.mapVerts(texChannel);
                        faceIndex = 0;
                        TVFace *tf = maxMesh.mapFaces(texChannel);
                        for (segmentPtr = geom->GetFirst(); segmentPtr; segmentPtr = segmentPtr->GetNext()) {
                            const dSubMesh& segment = segmentPtr->GetInfo();
                            int triangleCount = segment.m_indexCount / 3;
                            for (k = 0; k < triangleCount; k ++) {
                                for (int m = 0; m < 3; m ++) {
                                    int index = segment.m_indexes[k * 3 + m];
                                    UVVert v (dFloat (geom->m_uv1[index * 2 + 0]), dFloat (geom->m_uv1[index * 2 + 1]), 0.0f);
                                    tv[faceIndex * 3 + m] = v;
                                    tf[faceIndex].t[m] = faceIndex * 3 + m;
                                }
                                faceIndex ++;
                            }
                        }
                    }
                }
#endif
            } else {
                _ASSERTE (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; // thsi is a very big bug
	//relTirePos += chassisMatrix__.m_up.Scale ( suspensionLenght );
	relTirePos += m_localFrame.m_up.Scale ( suspensionLenght );

	m_tires[m_tiresCount].m_harpointInJointSpace  = m_localFrame.UntransformVector( relTirePos ); 
	m_tires[m_tiresCount].m_localAxisInJointSpace = dVector (0.0f, 0.0f, 1.0f, 0.0f);
	m_tires[m_tiresCount].m_tireAxelPositGlobal   = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_tireAxelVelocGlobal   = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_lateralPinGlobal      = dVector (0.0f, 0.0f, 0.0f, 1.0f);
	m_tires[m_tiresCount].m_longitudinalPinGlobal = 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);

	// set a default rolling resistance coefficient
	SetTireRollingResistance (m_tiresCount, DG_DEFAULT_ROLLING_RESISTANCE);

	m_tiresCount ++;
}
Esempio n. 3
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));
}
Esempio n. 4
0
int dComplemtaritySolver::BuildJacobianMatrix (int jointCount, dBilateralJoint** const jointArray, dFloat timestep, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray, int maxRowCount)
{
	int rowCount = 0;

	dParamInfo constraintParams;
	constraintParams.m_timestep = timestep;
	constraintParams.m_timestepInv = 1.0f / timestep;

	// calculate Jacobian derivative for each active joint	
	for (int j = 0; j < jointCount; j ++) {
		dBilateralJoint* const joint = jointArray[j];
		constraintParams.m_count = 0;
		joint->JacobianDerivative (&constraintParams); 

		int dofCount = constraintParams.m_count;
		joint->m_count = dofCount;
		joint->m_start = rowCount;

		// complete the derivative matrix for this joint
		int index = joint->m_start;
		dBodyState* const state0 = joint->m_state0;
		dBodyState* const state1 = joint->m_state1;

		const dMatrix& invInertia0 = state0->m_invInertia;
		const dMatrix& invInertia1 = state1->m_invInertia;

		dFloat invMass0 = state0->m_invMass;
		dFloat invMass1 = state1->m_invMass;
		dFloat weight = 0.9f;

		for (int i = 0; i < dofCount; i ++) {
			dJacobianPair* const row = &jacobianArray[index];
			dJacobianColum* const col = &jacobianColumnArray[index];
			jacobianArray[rowCount] = constraintParams.m_jacobians[i]; 

			dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
			dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
			dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
			dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);

			dVector tmpDiag (JMinvIM0linear.CompProduct(row->m_jacobian_IM0.m_linear) + JMinvIM0angular.CompProduct(row->m_jacobian_IM0.m_angular) + JMinvIM1linear.CompProduct(row->m_jacobian_IM1.m_linear) + JMinvIM1angular.CompProduct(row->m_jacobian_IM1.m_angular));
			dVector tmpAccel (JMinvIM0linear.CompProduct (state0->m_externalForce) + JMinvIM0angular.CompProduct(state0->m_externalTorque) + JMinvIM1linear.CompProduct (state1->m_externalForce) + JMinvIM1angular.CompProduct(state1->m_externalTorque));
			dFloat extenalAcceleration = -(tmpAccel[0] + tmpAccel[1] + tmpAccel[2]);

			col->m_diagDamp = 1.0f;
			col->m_coordenateAccel = constraintParams.m_jointAccel[i];
			col->m_jointLowFriction = constraintParams.m_jointLowFriction[i];
			col->m_jointHighFriction = constraintParams.m_jointHighFriction[i];

			col->m_deltaAccel = extenalAcceleration;
			col->m_coordenateAccel += extenalAcceleration;

			col->m_force = joint->m_jointFeebackForce[i] * weight;

			dFloat stiffness = COMPLEMENTARITY_PSD_DAMP_TOL * col->m_diagDamp;
			dFloat diag = (tmpDiag[0] + tmpDiag[1] + tmpDiag[2]);
			dAssert (diag > dFloat (0.0f));
			col->m_diagDamp = diag * stiffness;

			diag *= (dFloat(1.0f) + stiffness);
			col->m_invDJMinvJt = dFloat(1.0f) / diag;
			index ++;
			rowCount ++;
			dAssert (rowCount < maxRowCount);
		}
	}
	return rowCount;
}
Esempio n. 5
0
void dRayToRayCast (
	const dVector& ray_p0, const dVector& ray_p1, 
	const dVector& ray_q0, const dVector& ray_q1,
	dVector& pOut, dVector& qOut)
{
	dVector u (ray_p1 - ray_p0);
	dVector v (ray_q1 - ray_q0);
	dVector w (ray_p0 - ray_q0);

	dFloat a = u % u;        // always >= 0
	dFloat b = u % v;
	dFloat c = v % v;        // always >= 0
	dFloat d = u % w;
	dFloat e = v % w;
	dFloat D = a*c - b*b;   // always >= 0
	dFloat sD = D;			// sc = sN / sD, default sD = D >= 0
	dFloat tD = D;			// tc = tN / tD, default tD = D >= 0

	// compute the line parameters of the two closest points
	dFloat sN;
	dFloat tN;
	if (D < dFloat (1.0e-8f)) { 
		// the lines are almost parallel
		sN = dFloat (0.0f);  // force using point P0 on segment S1
		sD = dFloat (1.0f);  // to prevent possible division by 0.0 later
		tN = e;
		tD = c;
	} else {                
		// get the closest points on the infinite lines
		sN = (b*e - c*d);
		tN = (a*e - b*d);
		if (sN < dFloat (0.0f)) {       
			// sc < 0 => the s=0 edge is visible
			sN = dFloat (0.0f);
			tN = e;
			tD = c;
		}
		else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
			sN = sD;
			tN = e + b;
			tD = c;
		}
	}


	if (tN < dFloat (0.0f)) {           // tc < 0 => the t=0 edge is visible
		tN = dFloat (0.0f);
		// recompute sc for this edge
		if (-d < dFloat (0.0f))
			sN = dFloat (0.0f);
		else if (-d > a)
			sN = sD;
		else {
			sN = -d;
			sD = a;
		}
	}
	else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
		tN = tD;
		// recompute sc for this edge
		if ((-d + b) < dFloat (0.0f))
			sN = dFloat (0.0f);
		else if ((-d + b) > a)
			sN = sD;
		else {
			sN = (-d + b);
			sD = a;
		}
	}

	// finally do the division to get sc and tc
	dFloat sc = (dAbs(sN) < dFloat(1.0e-8f) ? dFloat (0.0f) : sN / sD);
	dFloat tc = (dAbs(tN) < dFloat(1.0e-8f) ? dFloat (0.0f) : tN / tD);

	pOut = ray_p0 + u.Scale (sc);
	qOut = ray_q0 + v.Scale (tc);
}
Esempio n. 6
0
void SerializeMesh (const NewtonMesh* const mesh, TiXmlElement* const rootNode)
{
	TiXmlElement* pointElement = new TiXmlElement ("points");
	rootNode->LinkEndChild(pointElement);

	int bufferCount = dMax (NewtonMeshGetVertexCount(mesh), NewtonMeshGetPointCount(mesh));
	int bufferSizeInBytes = bufferCount * sizeof (dFloat) * 4 * 12;
	char* const buffer = new char[bufferSizeInBytes];
	dFloat* const packVertex = new dFloat [4 * bufferCount];

	int vertexCount = NewtonMeshGetVertexCount (mesh); 
	int vertexStride = NewtonMeshGetVertexStrideInByte(mesh) / sizeof (dFloat64);
	const dFloat64* const vertex = NewtonMeshGetVertexArray(mesh); 

	// pack the vertex Array
	int* const vertexIndexList = new int [vertexCount];
	for (int i = 0; i < vertexCount; i ++) {
		packVertex[i * 4 + 0] = dFloat(vertex[i * vertexStride + 0]);
		packVertex[i * 4 + 1] = dFloat(vertex[i * vertexStride + 1]);
		packVertex[i * 4 + 2] = dFloat(vertex[i * vertexStride + 2]);
		packVertex[i * 4 + 3] = dFloat(vertex[i * vertexStride + 3]);
		vertexIndexList[i] = i;
	}
	dFloatArrayToString (packVertex, vertexCount * 4, buffer, bufferSizeInBytes);

	TiXmlElement* const position = new TiXmlElement ("position");
	pointElement->LinkEndChild(position);
	position->SetAttribute("float4", vertexCount);
	position->SetAttribute("floats", buffer);

	// pack the normal array
	int pointCount = NewtonMeshGetPointCount (mesh); 
	int pointStride = NewtonMeshGetPointStrideInByte(mesh) / sizeof (dFloat64);
	const dFloat64* const normals = NewtonMeshGetNormalArray(mesh); 
	int* const normalIndexList = new int [pointCount];
	for (int i = 0; i < pointCount; i ++) {
		packVertex[i * 3 + 0] = dFloat(normals[i * pointStride + 0]);
		packVertex[i * 3 + 1] = dFloat(normals[i * pointStride + 1]);
		packVertex[i * 3 + 2] = dFloat(normals[i * pointStride + 2]);
	}
	int count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, normalIndexList);
	dFloatArrayToString (packVertex, count * 3, buffer, bufferSizeInBytes);

	TiXmlElement* const normal = new TiXmlElement ("normal");
	pointElement->LinkEndChild(normal);
	normal->SetAttribute("float3", count);
	normal->SetAttribute("floats", buffer);

	// pack the uv0 array
	int* const uv0IndexList = new int [pointCount];
	const dFloat64* const uv0s = NewtonMeshGetUV0Array(mesh); 
	for (int i = 0; i < pointCount; i ++) {
		packVertex[i * 3 + 0] = dFloat(uv0s[i * pointStride + 0]);
		packVertex[i * 3 + 1] = dFloat(uv0s[i * pointStride + 1]);
		packVertex[i * 3 + 2] = 0.0f;
	}
	count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, uv0IndexList);
	for (int i = 0; i < pointCount; i ++) {
		packVertex[i * 2 + 0] = packVertex[i * 3 + 0];
		packVertex[i * 2 + 1] = packVertex[i * 3 + 1];
	}
	dFloatArrayToString (packVertex, count * 2, buffer, bufferSizeInBytes);

	TiXmlElement* const uv0 = new TiXmlElement ("uv0");
	pointElement->LinkEndChild(uv0);
	uv0->SetAttribute("float2", count);
	uv0->SetAttribute("floats", buffer);

	// pack the uv1 array
	int* const uv1IndexList = new int [pointCount];
	const dFloat64* const uv1s = NewtonMeshGetUV1Array(mesh); 
	for (int i = 0; i < pointCount; i ++) {
		packVertex[i * 3 + 0] = dFloat(uv1s[i * pointStride + 0]);
		packVertex[i * 3 + 1] = dFloat(uv1s[i * pointStride + 1]);
		packVertex[i * 3 + 2] = 0.0f;
	}
	count = dPackVertexArray (packVertex, 3, 3 * sizeof (dFloat), pointCount, uv1IndexList);
	for (int i = 0; i < pointCount; i ++) {
		packVertex[i * 2 + 0] = packVertex[i * 3 + 0];
		packVertex[i * 2 + 1] = packVertex[i * 3 + 1];
	}
	dFloatArrayToString (packVertex, count * 2, buffer, bufferSizeInBytes);

	TiXmlElement* const uv1 = new TiXmlElement ("uv1");
	pointElement->LinkEndChild(uv1);
	uv1->SetAttribute("float2", count);
	uv1->SetAttribute("floats", buffer);

	// save the polygon array
	int faceCount = NewtonMeshGetTotalFaceCount (mesh); 
	int indexCount = NewtonMeshGetTotalIndexCount (mesh); 

	int* const faceArray = new int [faceCount];
	void** const indexArray = new void* [indexCount];
	int* const materialIndexArray = new int [faceCount];
	int* const remapedIndexArray = new int [indexCount];

	NewtonMeshGetFaces (mesh, faceArray, materialIndexArray, indexArray); 

	// save the faces vertex Count
	dIntArrayToString (faceArray, faceCount, buffer, bufferSizeInBytes);
	TiXmlElement* const polygons = new TiXmlElement ("polygons");
	rootNode->LinkEndChild(polygons);
	polygons->SetAttribute("count", faceCount);
	polygons->SetAttribute("faceIndexCount", buffer);

	dIntArrayToString (materialIndexArray, faceCount, buffer, bufferSizeInBytes);
	TiXmlElement* const faceMaterial = new TiXmlElement ("faceMaterial");
	polygons->LinkEndChild(faceMaterial);
	faceMaterial->SetAttribute("index", buffer);

	for (int i = 0; i < indexCount; i ++) {
		int index = NewtonMeshGetVertexIndex (mesh, indexArray[i]);
		remapedIndexArray[i] = vertexIndexList[index];
	}
	dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
	TiXmlElement* const positionIndex = new TiXmlElement ("position");
	polygons->LinkEndChild(positionIndex);
	positionIndex->SetAttribute("index", buffer);


	for (int i = 0; i < indexCount; i ++) {
		int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
		remapedIndexArray[i] = normalIndexList[index];
	}
	dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
	TiXmlElement* const normalIndex = new TiXmlElement ("normal");
	polygons->LinkEndChild(normalIndex);
	normalIndex->SetAttribute("index", buffer);

	for (int i = 0; i < indexCount; i ++) {
		int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
		remapedIndexArray[i] = uv0IndexList[index];
	}
	dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
	TiXmlElement* const uv0Index = new TiXmlElement ("uv0");
	polygons->LinkEndChild(uv0Index);
	uv0Index->SetAttribute("index", buffer);

	for (int i = 0; i < indexCount; i ++) {
		int index = NewtonMeshGetPointIndex(mesh, indexArray[i]);
		remapedIndexArray[i] = uv1IndexList[index];
	}
	dIntArrayToString (remapedIndexArray, indexCount, buffer, bufferSizeInBytes);
	TiXmlElement* const uv1Index = new TiXmlElement ("uv1");
	polygons->LinkEndChild(uv1Index);
	uv1Index->SetAttribute("index", buffer);

	delete[] remapedIndexArray;
	delete[] faceArray;
	delete[] indexArray;
	delete[] materialIndexArray;
	delete[] uv1IndexList;
	delete[] uv0IndexList;
	delete[] normalIndexList;
	delete[] vertexIndexList;
	delete[] packVertex;
	delete[] buffer;
}
Esempio n. 7
0
void CustomVehicleController::Finalize()
{
	dWeightDistibutionSolver solver;
	dFloat64 unitAccel[VEHICLE_CONTROLLER_MAX_JOINTS];
	dFloat64 sprungMass[VEHICLE_CONTROLLER_MAX_JOINTS];


	// make sure tire are aligned
/*
	memset (unitAccel, sizeof (unitAccel), 0);
	int index = 0;
	for (TireList::dListNode* node0 = m_tireList.GetFirst(); node0; node0 = node0->GetNext()) {
		if (unitAccel[index] == 0) {
			CustomVehicleControllerBodyStateTire* const tire0 = &node0->GetInfo();
			for (TireList::dListNode* node1 = node0->GetNext(); node1; node1 = node1->GetNext()) {
				CustomVehicleControllerBodyStateTire* const tire1 = &node->GetInfo();
			}
		}
	}
*/
	
	int count = 0;
	m_chassisState.m_matrix = dGetIdentityMatrix();
	m_chassisState.UpdateInertia();
	dVector dir (m_chassisState.m_localFrame[1]);
	for (dTireList::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
		CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
		dVector posit  (tire->m_localFrame.m_posit);  
		dComplemtaritySolver::dJacobian &jacobian0 = solver.m_jacobians[count];
		dComplemtaritySolver::dJacobian &invMassJacobian0 = solver.m_invMassJacobians[count];
		jacobian0.m_linear = dir;
		jacobian0.m_angular = posit * dir;
		jacobian0.m_angular.m_w = 0.0f;

		invMassJacobian0.m_linear = jacobian0.m_linear.Scale(m_chassisState.m_invMass);
		invMassJacobian0.m_angular = jacobian0.m_angular.CompProduct(m_chassisState.m_localInvInertia);

		dFloat diagnal = jacobian0.m_linear % invMassJacobian0.m_linear + jacobian0.m_angular % invMassJacobian0.m_angular;
		solver.m_diagRegularizer[count] = diagnal * 0.005f;
		solver.m_invDiag[count] = 1.0f / (diagnal + solver.m_diagRegularizer[count]);

		unitAccel[count] = 1.0f;
		sprungMass[count] = 0.0f;
		count ++;
	}
	if (count) {
		solver.m_count = count;
		solver.Solve (count, 1.0e-6f, sprungMass, unitAccel);
	}

	int index = 0;
	for (dTireList::dListNode* node = m_tireList.GetFirst(); node; node = node->GetNext()) {
		CustomVehicleControllerBodyStateTire* const tire = &node->GetInfo();
		tire->m_restSprunMass = dFloat (5.0f * dFloor (sprungMass[index] / 5.0f + 0.5f));
		if (m_engine) {
			tire->CalculateRollingResistance (m_engine->GetTopSpeed());
		}
		index ++;
	}

	m_sleepCounter = VEHICLE_SLEEP_COUNTER;
	m_finalized = true;
}
dVector dCustomPlayerController::CalculateImpulse(
	int rows, 
	const dFloat* const rhs,
	const dFloat* const low,
	const dFloat* const high,
	const int* const normalIndex,
	const dComplementaritySolver::dJacobian* const jt) const
{
	dMatrix invInertia;
	dFloat impulseMag[D_MAX_ROWS];
	dFloat massMatrix[D_MAX_ROWS][D_MAX_ROWS];

	NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);
	for (int i = 0; i < rows; i++) {
		dComplementaritySolver::dJacobian jInvMass(jt[i]);

		jInvMass.m_linear = jInvMass.m_linear.Scale (m_invMass);
		jInvMass.m_angular = invInertia.RotateVector (jInvMass.m_angular);

		dVector tmp(jInvMass.m_linear * jt[i].m_linear + jInvMass.m_angular * jt[i].m_angular);

		dFloat a00 = (tmp.m_x + tmp.m_y + tmp.m_z) * 1.0001f;
		massMatrix[i][i] = a00;

		impulseMag[i] = 0.0f;
		for (int j = i + 1; j < rows; j++) {
			dVector tmp1(jInvMass.m_linear * jt[j].m_linear + jInvMass.m_angular * jt[j].m_angular);
			dFloat a01 = tmp1.m_x + tmp1.m_y + tmp1.m_z;
			massMatrix[i][j] = a01;
			massMatrix[j][i] = a01;
		}
	}

	dGaussSeidelLcpSor(rows, D_MAX_ROWS, &massMatrix[0][0], impulseMag, rhs, normalIndex, low, high, dFloat(1.0e-2f), 32, dFloat(1.1f));

	dVector netImpulse(0.0f);
	for (int i = 0; i < rows; i++) {
		netImpulse += jt[i].m_linear.Scale(impulseMag[i]);
	}
	return netImpulse;
}
int dCustomPlayerController::ResolveInterpenetrations(int contactCount, NewtonWorldConvexCastReturnInfo* const contactArray)
{
	dVector zero (0.0f);
	dVector veloc (0.0f);
	dVector savedVeloc (0.0f);

	NewtonBodyGetVelocity(m_kinematicBody, &savedVeloc[0]);
	NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);

	dFloat timestep = 0.1f;
	dFloat invTimestep = 1.0f / timestep;

	dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
	dFloat rhs[D_MAX_ROWS];
	dFloat low[D_MAX_ROWS];
	dFloat high[D_MAX_ROWS];
	int normalIndex[D_MAX_ROWS];

	NewtonWorld* const world = m_manager->GetWorld();
	for (int i = 0; i < 3; i++) {
		jt[i].m_linear = zero;
		jt[i].m_angular = zero;
		jt[i].m_angular[i] = dFloat(1.0f);
		rhs[i] = 0.0f;
		low[i] = -1.0e12f;
		high[i] = 1.0e12f;
		normalIndex[i] = 0;
	}

	dFloat penetration = D_MAX_COLLISION_PENETRATION * 10.0f;
	for (int j = 0; (j < 8) && (penetration > D_MAX_COLLISION_PENETRATION) ; j ++) {
		dMatrix matrix;
		dVector com(0.0f);
		NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
		NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
		com = matrix.TransformVector(com);
		com.m_w = 0.0f;

		int rowCount = 3;
		for (int i = 0; i < contactCount; i++) {
			NewtonWorldConvexCastReturnInfo& contact = contactArray[i];

			dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
			dVector normal(contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);

			jt[rowCount].m_linear = normal;
			jt[rowCount].m_angular = (point - com).CrossProduct(normal);

			low[rowCount] = 0.0f;
			high[rowCount] = 1.0e12f;
			normalIndex[rowCount] = 0;
			penetration = dClamp(contact.m_penetration - D_MAX_COLLISION_PENETRATION * 0.5f, dFloat(0.0f), dFloat(0.5f));
			rhs[rowCount] = penetration * invTimestep;
			rowCount++;
		}

		dVector impulse (CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));

		impulse = impulse.Scale(m_invMass);
		NewtonBodySetVelocity(m_kinematicBody, &impulse[0]);
		NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);

		NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
		NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);

		contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, contactArray, 4, 0);

		penetration = 0.0f;
		for (int i = 0; i < contactCount; i++) {
			penetration = dMax(contactArray[i].m_penetration, penetration);
		}
	}

	NewtonBodySetVelocity(m_kinematicBody, &savedVeloc[0]);
	return contactCount;
}
// a pseudo Gaussian random with mean 0 and variance 0.5f
dFloat dGaussianRandom (dFloat amp)
{
	unsigned val;
	val = dRand() + dRand();
	return amp * (dFloat (val) / dFloat(dRAND_MAX) - 1.0f) * 0.5f;
}
void dCustomPlayerController::ResolveStep(dFloat timestep)
{
	dMatrix matrix;
	dMatrix stepMatrix;
	dVector veloc(0.0f);
	dVector zero(0.0f);
	NewtonWorldConvexCastReturnInfo info[16];

	NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
	NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);

	dMatrix coodinateMatrix (m_localFrame * matrix);

	dComplementaritySolver::dJacobian jt[3];
	dFloat rhs[3];
	dFloat low[3];
	dFloat high[3];
	int normalIndex[3];

	jt[0].m_linear = coodinateMatrix[0];
	jt[0].m_angular = zero;
	low[0] = 0.0f;
	high[0] = 1.0e12f;
	normalIndex[0] = 0;
	rhs[0] = -m_impulse.DotProduct3(jt[0].m_linear) * m_invMass;

	// add lateral traction friction
	jt[1].m_linear = coodinateMatrix[1];
	jt[1].m_angular = zero;
	low[1] = -m_friction;
	high[1] = m_friction;
	normalIndex[1] = -1;
	dVector tmp1(veloc * jt[1].m_linear);
	rhs[1] = -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);

	// add longitudinal  traction friction
	jt[2].m_linear = coodinateMatrix[2];
	jt[2].m_angular = zero;
	low[2] = -m_friction;
	high[2] = m_friction;
	normalIndex[2] = -2;
	dVector tmp2(veloc * jt[2].m_linear);
	rhs[2] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
	
	dVector impulse(veloc.Scale(m_mass) + CalculateImpulse(3, rhs, low, high, normalIndex, jt));

	impulse = impulse.Scale(m_invMass);
	NewtonBodySetVelocity(m_kinematicBody, &impulse[0]);
	NewtonBodyIntegrateVelocity(m_kinematicBody, timestep);

	NewtonWorld* const world = m_manager->GetWorld();
	NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);
	
	NewtonBodyGetMatrix(m_kinematicBody, &stepMatrix[0][0]);
	int contactCount = NewtonWorldCollide(world, &stepMatrix[0][0], shape, this, PrefilterCallback, info, 4, 0);

	NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
	NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);

	dFloat maxHigh = 0.0f;
	for (int i = 0; i < contactCount; i++) {
		NewtonWorldConvexCastReturnInfo& contact = info[i];
		dVector point(contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
		point = m_localFrame.UntransformVector (stepMatrix.UntransformVector(point));
		maxHigh = dMax (point.m_x, maxHigh);
	}
	if ((maxHigh < m_stepHeight) && (maxHigh > m_contactPatch)) {
		dVector step (stepMatrix.RotateVector(m_localFrame.RotateVector (dVector(maxHigh, dFloat(0.0f), dFloat(0.0f), dFloat(0.0f)))));
		matrix.m_posit += step;
		NewtonBodySetMatrix(m_kinematicBody, &matrix[0][0]);
	}
}
void dNewtonJointDoubleHingeActuator::SetTargetAngle0(dFloat angle, dFloat minLimit, dFloat maxLimit)
{
	dCustomDoubleHingeActuator* const joint = (dCustomDoubleHingeActuator*) m_joint;
	joint->SetLimits(dMin(minLimit * DEGREES_TO_RAD, dFloat(0.0f)), dMax(maxLimit * DEGREES_TO_RAD, dFloat(0.0f)));
	joint->SetTargetAngle0(dClamp (angle * DEGREES_TO_RAD, joint->GetMinAngularLimit0(), joint->GetMaxAngularLimit0()));
}
Esempio n. 13
0
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep)
{
	// update the camera;
	DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);

	dMatrix targetMatrix (m_camera->GetNextMatrix());

	int mouseX;
	int mouseY;
	scene->GetMousePosition (mouseX, mouseY);

	// slow down the Camera if we have a Body
	dFloat slowDownFactor = scene->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f;

	// do camera translation
	if (scene->GetKeyState ('W')) {
		targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
	}
	if (scene->GetKeyState ('S')) {
		targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
	}
	if (scene->GetKeyState ('A')) {
		targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
	}
	if (scene->GetKeyState ('D')) {
		targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
	}

	if (scene->GetKeyState ('Q')) {
		targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
	}

	if (scene->GetKeyState ('E')) {
		targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
	}

	// do camera rotation, only if we do not have anything picked
	bool buttonState = m_mouseLockState || scene->GetMouseKeyState(0);
	if (!m_targetPicked && buttonState) {
		int mouseSpeedX = mouseX - m_mousePosX;
		int mouseSpeedY = mouseY - m_mousePosY;

		if (mouseSpeedX > 0) {
			m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f);
		} else if (mouseSpeedX < 0){
			m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f);
		}

		if (mouseSpeedY > 0) {
			m_pitch += m_pitchRate;
		} else if (mouseSpeedY < 0){
			m_pitch -= m_pitchRate;
		}
		m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f));
	}

	m_mousePosX = mouseX;
	m_mousePosY = mouseY;

	dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw));
	dQuaternion rot (matrix);
	m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit);

	UpdatePickBody(scene, timestep);
}
Esempio n. 14
0
void DemoCameraListener::UpdatePickBody(DemoEntityManager* const scene, dFloat timestep) 
{
	// handle pick body from the screen
	bool mousePickState = scene->GetMouseKeyState(0);
	if (!m_targetPicked) {
		if (!m_prevMouseState && mousePickState) {
			dFloat param;
			dVector posit;
			dVector normal;

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

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

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

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

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

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

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

	m_prevMouseState = mousePickState;
}
void dRuntimeProfiler::RenderThreadPerformance ()
{
	NewtonWorld* const world = m_scene->GetNewton(); 
	int threadCount = NewtonGetThreadsCount(world);

	if (threadCount > 0) {
		struct GLViewPort
		{
			GLint x;
			GLint y;
			GLsizei width;
			GLsizei height;
		} viewport;

		//Retrieves the viewport and stores it in the variable
		glGetIntegerv(GL_VIEWPORT, &viewport.x); 


		glColor3f(1.0, 1.0, 1.0);
		glDisable (GL_LIGHTING);
		glDisable(GL_TEXTURE_2D);

		glMatrixMode(GL_TEXTURE);
		glPushMatrix();
		glLoadIdentity();

		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		glLoadIdentity();
		gluOrtho2D(0, viewport.width, 0, viewport.height );

		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();

		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		glDisable(GL_DEPTH_TEST);
		glDisable(GL_BLEND);

		m_width = viewport.width;
		m_height = viewport.height;

		float x0 = m_width - 400.0f; 
		float x1 = x0 + 256.0f;
		float y0 = 50.0f;
		for (int i = 0; i < threadCount; i ++) {
			char label[32];
			sprintf (label, "thread %d", i);
			DrawLabel (x0 - 50, y0 + i * 20 + 5, label);
		}

		DrawLabel (x0, y0 - 20, "0.0 ms");
		DrawLabel ((x1 + x0) * 0.5f, y0 - 20, "8.33 ms");
		DrawLabel (x1, y0 - 20, "16.66 ms");

		y0 -= 15.0f;
		dVector color (1.0f, 0.0f, 0.0f, 1.0f/8.0f);
		DrawRectangle (x0, m_height - (y0 + 20.0f * threadCount), x1 - x0, 20 * threadCount, color);

		color = dVector (1.0f, 1.0f, 0.0f, 1.0f/2.0f);
		if (threadCount > 1)  {
			for (int i = 0; i < threadCount; i ++) {
				int tick = NewtonReadThreadPerformanceTicks (world, i);
				dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
				DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
				y0 += 20.0f;
			}
		} else {
			int tick =  NewtonReadPerformanceTicks (world, NEWTON_PROFILER_WORLD_UPDATE);
			dFloat time = dFloat (tick) * (1.0e-3f * 256.0f / 16.666f);
			DrawRectangle(x0, m_height - (y0 + 15), time, 10, color);
		}

		glMatrixMode(GL_TEXTURE);
		glPopMatrix();

		glMatrixMode(GL_PROJECTION);
		glPopMatrix();

		glMatrixMode(GL_MODELVIEW);
		glPopMatrix();

		glDisable(GL_BLEND);
		glEnable (GL_DEPTH_TEST);
		glColor3f(1.0, 1.0, 1.0);
	}
}
void dCustomPlayerController::ResolveCollision()
{
	dMatrix matrix;
	NewtonWorldConvexCastReturnInfo info[D_MAX_ROWS];
	NewtonWorld* const world = m_manager->GetWorld();

	NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
	NewtonCollision* const shape = NewtonBodyGetCollision(m_kinematicBody);

	int contactCount = NewtonWorldCollide(world, &matrix[0][0], shape, this, PrefilterCallback, info, 4, 0);
	if (!contactCount) {
		return;
	}

	dFloat maxPenetration = 0.0f;
	for (int i = 0; i < contactCount; i ++) {
		maxPenetration = dMax (info[i].m_penetration, maxPenetration);
	}

	if (maxPenetration > D_MAX_COLLISION_PENETRATION) {
		ResolveInterpenetrations(contactCount, info);
		NewtonBodyGetMatrix(m_kinematicBody, &matrix[0][0]);
	}
	
	int rowCount = 0;
	dVector zero(0.0f);

	dMatrix invInertia;
	dVector com(0.0f);
	dVector veloc(0.0f);
	dComplementaritySolver::dJacobian jt[D_MAX_ROWS];
	dFloat rhs[D_MAX_ROWS];
	dFloat low[D_MAX_ROWS];
	dFloat high[D_MAX_ROWS];
	dFloat impulseMag[D_MAX_ROWS];
	int normalIndex[D_MAX_ROWS];
	
	NewtonBodyGetVelocity(m_kinematicBody, &veloc[0]);
	NewtonBodyGetCentreOfMass(m_kinematicBody, &com[0]);
	NewtonBodyGetInvInertiaMatrix(m_kinematicBody, &invInertia[0][0]);

//	const dMatrix localFrame (dPitchMatrix(m_headingAngle) * m_localFrame * matrix);
	const dMatrix localFrame (m_localFrame * matrix);

	com = matrix.TransformVector(com);
	com.m_w = 0.0f;
	for (int i = 0; i < contactCount; i ++) {
		NewtonWorldConvexCastReturnInfo& contact = info[i];

		dVector point (contact.m_point[0], contact.m_point[1], contact.m_point[2], 0.0f);
		dVector normal (contact.m_normal[0], contact.m_normal[1], contact.m_normal[2], 0.0f);


		jt[rowCount].m_linear = normal;
		jt[rowCount].m_angular = (point - com).CrossProduct(normal);

		low[rowCount] = 0.0f;
		high[rowCount] = 1.0e12f;
		normalIndex[rowCount] = 0;
		dVector tmp (veloc * jt[rowCount].m_linear.Scale (1.001f));
		rhs[rowCount] = - (tmp.m_x + tmp.m_y + tmp.m_z);
		rowCount ++;
		dAssert (rowCount < (D_MAX_ROWS - 3));

		//dFloat updir = localFrame.m_front.DotProduct3(normal);
		dFloat friction = m_manager->ContactFriction(this, point, normal, contact.m_hitBody);
		if (friction > 0.0f)
		{
			// add lateral traction friction
			dVector sideDir (localFrame.m_up.CrossProduct(normal).Normalize());

			jt[rowCount].m_linear = sideDir;
			jt[rowCount].m_angular = (point - com).CrossProduct(sideDir);

			low[rowCount] = -friction;
			high[rowCount] = friction;
			normalIndex[rowCount] = -1;

			dVector tmp1 (veloc * jt[rowCount].m_linear);
			rhs[rowCount] =  -m_lateralSpeed - (tmp1.m_x + tmp1.m_y + tmp1.m_z);
			rowCount++;
			dAssert (rowCount < (D_MAX_ROWS - 3));

			// add longitudinal  traction friction
			dVector frontDir (normal.CrossProduct(sideDir));
			jt[rowCount].m_linear = frontDir;
			jt[rowCount].m_angular = (point - com).CrossProduct(frontDir);

			low[rowCount] = -friction;
			high[rowCount] = friction;
			normalIndex[rowCount] = -2;
			dVector tmp2 (veloc * jt[rowCount].m_linear);
			rhs[rowCount] = -m_forwardSpeed - (tmp2.m_x + tmp2.m_y + tmp2.m_z);
			rowCount++;
			dAssert(rowCount < (D_MAX_ROWS - 3));
		}
	}

	for (int i = 0; i < 3; i++) {
		jt[rowCount].m_linear = zero;
		jt[rowCount].m_angular = zero;
		jt[rowCount].m_angular[i] = dFloat(1.0f);
		rhs[rowCount] = 0.0f;
		impulseMag[rowCount] = 0;
		low[rowCount] = -1.0e12f;
		high[rowCount] = 1.0e12f;
		normalIndex[rowCount] = 0;

		rowCount ++;
		dAssert (rowCount < D_MAX_ROWS);
	}

	dVector impulse (veloc.Scale (m_mass) + CalculateImpulse(rowCount, rhs, low, high, normalIndex, jt));
	veloc = impulse.Scale(m_invMass);
	NewtonBodySetVelocity(m_kinematicBody, &veloc[0]);
}
void dRuntimeProfiler::Render (int mask)
{
	struct GLViewPort
	{
		GLint x;
		GLint y;
		GLsizei width;
		GLsizei height;
	} viewport;

	//Retrieves the viewport and stores it in the variable
	glGetIntegerv(GL_VIEWPORT, &viewport.x); 

	m_width = viewport.width;
	m_height = viewport.height;

	NewtonWorld* const world = m_scene->GetNewton(); 
	for (int i = 0; i < MAX_TRACKS; i ++) {
		m_perfomanceTracks[i][m_frameIndex] = NewtonReadPerformanceTicks (world, i);
	}

	glColor3f(1.0, 1.0, 1.0);
	glDisable (GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);

	glMatrixMode(GL_TEXTURE);
	glPushMatrix();
	glLoadIdentity();

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0, viewport.width, 0, viewport.height );

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glDisable(GL_DEPTH_TEST);
	glDisable(GL_BLEND);


	dFloat x0 = dFloat (m_oringin_x);
	dFloat y0 = dFloat (m_oringin_y);
	dFloat x1 = x0 + MAX_FRAMES * MAX_FRAMES_STEP;
	dFloat y1 = y0 + CHART_HIEGHT;


	glBegin(GL_LINES);

	glVertex3f (x0, y0, 0.0f);
	glVertex3f (x0, y1, 0.0f);

	glVertex3f (x0, y0, 0.0f);
	glVertex3f (x1, y0, 0.0f);


	for (int i = 1; i < 4; i ++) {
		dFloat y = y0 + (y1 - y0) * i / 4;
		glVertex3f (x0 - 5, y, 0.0f);
		glVertex3f (x0 + 5, y, 0.0f);
	}

	for (int i = 1; i < MAX_FRAMES; i += 16) {
		dFloat x = x0 + (x1 - x0) * i / MAX_FRAMES;
		glVertex3f (x , y0 - 5, 0.0f);
		glVertex3f (x , y0 + 5, 0.0f);
	}
	glEnd();

	DrawLabel (10, m_height  - m_nextLine, "Profiler legend");
	m_nextLine += 20;

	// total engine time
	if (mask & 1) {
		DrawLabel (10, m_height - m_nextLine, "white chart: world global update");
		DrawTrack (x0, y0, dVector (1.0f, 1.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_WORLD_UPDATE][0]);
		m_nextLine += 20;
	}


	// draw collision performance
	if (mask & 2) { 
		DrawLabel (10, m_height - m_nextLine, "red chart: collision global update");
		DrawTrack (x0, y0, dVector (1.0f, 0.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE][0]);
		m_nextLine += 20;
	}

	if (mask & 4) { 
		DrawLabel (10, m_height - m_nextLine, "green chart: collision broad phase update");
		DrawTrack (x0, y0, dVector (0.0f, 1.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE_BROAD_PHASE][0]);
		m_nextLine +=  20;
	}

	if (mask & 8) { 
		DrawLabel (10, m_height - m_nextLine, "blue chart: collision narrow phase update");
		DrawTrack (x0, y0, dVector (0.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_COLLISION_UPDATE_NARROW_PHASE][0]);
		m_nextLine += 20;
	}

	// draw dynamics performance
	if (mask & 16) { 
		DrawLabel (10, m_height - m_nextLine, "cyan chart: dynamics global update");
		DrawTrack (x0, y0, dVector (0.0f, 1.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_UPDATE][0]);
		m_nextLine += 20;
	}

	if (mask & 32) { 
		DrawLabel (10, m_height - m_nextLine, "black chart: dynamics solver update");
		DrawTrack (x0, y0, dVector (0.0f, 0.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_CONSTRAINT_GRAPH][0]);
		m_nextLine += 20;
	}

	if (mask & 64) { 
		DrawLabel (10, m_height - m_nextLine, "yellow chart: dynamics solver update");
		DrawTrack (x0, y0, dVector (1.0f, 1.0f, 0.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_DYNAMICS_SOLVE_CONSTRAINT_GRAPH][0]);
		m_nextLine += 20;
	}

	// draw force Update performance
	if (mask & 128) { 
		DrawLabel (10, m_height - m_nextLine, "magenta chart: force and torque callback update");
		DrawTrack (x0, y0, dVector (1.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_FORCE_CALLBACK_UPDATE][0]);
		m_nextLine += 20;
	}

	if (mask & 256) { 
		DrawLabel (10, m_height - m_nextLine, "magenta chart: pre simulation listener");
		DrawTrack (x0, y0, dVector (1.0f, 0.0f, 1.0f), m_frameIndex, &m_perfomanceTracks[NEWTON_PROFILER_FORCE_CALLBACK_UPDATE][0]);
		m_nextLine += 20;
	}

	if (mask & 256) { 
		DrawLabel (10, m_height - m_nextLine, "purple chart: pre simulation listener");
		DrawTrack (x0, y0, dVector (0.64f, 0.29f, 0.64f), m_frameIndex, &m_perfomanceTracks[NEWTON_PRE_LISTERNER_CALLBACK_UPDATE][0]);
		m_nextLine += 20;
	}

	if (mask & 512) { 
		DrawLabel (10, m_height - m_nextLine, "pink chart: post simulation listener");
		DrawTrack (x0, y0, dVector (1.0f, 0.68f, 0.79f), m_frameIndex, &m_perfomanceTracks[NEWTON_POST_LISTERNER_CALLBACK_UPDATE][0]);
		m_nextLine += 20;
	}

	{
		glColor3f(1.0, 1.0, 1.0);
		DrawLabel (x0 - 20, y0 + (y1 - y0) * 0 / 4, "0");

		for (int i = 1; i < 5; i ++) {
			char label[32];
			sprintf (label, "%4.2f", (1000.0f / 60.0f) * (float)i / 4.0f );
			DrawLabel (x0 - 42, y0 + (y1 - y0) * i / 4, label);
		}
	}

	glMatrixMode(GL_TEXTURE);
	glPopMatrix();

	glMatrixMode(GL_PROJECTION);
	glPopMatrix();

	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();

	glEnable( GL_DEPTH_TEST );

	m_frameIndex = (m_frameIndex + 1) % MAX_FRAMES;

	glColor3f(1.0, 1.0, 1.0);
}
void dAnimationRigHinge::JacobianDerivative(dComplementaritySolver::dParamInfo* const constraintParams)
{
	m_rowAccel = 0.0f;
	NewtonImmediateModeConstraint descriptor;
	NewtonJoint* const newtonJoint = dCustomHinge::GetJoint();
	int rows = NewtonUserJointSubmitImmediateModeConstraint(newtonJoint, &descriptor, constraintParams->m_timestep);
	dAssert (rows == 6);

	for (int i = 0; i < 5; i ++) {
		constraintParams->m_jacobians[i].m_jacobian_J01.m_linear = dVector (descriptor.m_jacobian01[i][0], descriptor.m_jacobian01[i][1], descriptor.m_jacobian01[i][2], dFloat (0.0f));
		constraintParams->m_jacobians[i].m_jacobian_J01.m_angular = dVector (descriptor.m_jacobian01[i][3], descriptor.m_jacobian01[i][4], descriptor.m_jacobian01[i][5], dFloat (0.0f));
		constraintParams->m_jacobians[i].m_jacobian_J10.m_linear = dVector(descriptor.m_jacobian10[i][0], descriptor.m_jacobian10[i][1], descriptor.m_jacobian10[i][2], dFloat(0.0f));
		constraintParams->m_jacobians[i].m_jacobian_J10.m_angular = dVector(descriptor.m_jacobian10[i][3], descriptor.m_jacobian10[i][4], descriptor.m_jacobian10[i][5], dFloat(0.0f));
		constraintParams->m_jointAccel[i] = descriptor.m_jointAccel[i];
		constraintParams->m_jointLowFrictionCoef[i] = descriptor.m_minFriction[i];
		constraintParams->m_jointHighFrictionCoef[i] = descriptor.m_maxFriction[i];
		constraintParams->m_normalIndex[i] = 0;
	}

	m_jacobial01.m_linear = dVector (descriptor.m_jacobian01[5][0], descriptor.m_jacobian01[5][1], descriptor.m_jacobian01[5][2], dFloat (0.0f));
	m_jacobial01.m_angular = dVector (descriptor.m_jacobian01[5][3], descriptor.m_jacobian01[5][4], descriptor.m_jacobian01[5][5], dFloat (0.0f));

	m_jacobial10.m_linear = dVector(descriptor.m_jacobian10[5][0], descriptor.m_jacobian10[5][1], descriptor.m_jacobian10[5][2], dFloat(0.0f));
	m_jacobial10.m_angular = dVector(descriptor.m_jacobian10[5][3], descriptor.m_jacobian10[5][4], descriptor.m_jacobian10[5][5], dFloat(0.0f));

	constraintParams->m_count = 5;
}
Esempio n. 19
0
dFloat Rand (dFloat base)
{
	return 2.0f * base * (dFloat(rand()) / RAND_MAX - 0.5f); 
}
void dCustomBallAndSocket::SetConeLimits(dFloat maxAngle)
{
	m_maxConeAngle = dMin (dAbs (maxAngle), dFloat (160.0f * dDegreeToRad));
}
void CustomControlledBallAndSocket::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 tree orthonormal direction
	NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]);
	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]);

#if 0
	dVector euler0;
	dVector euler1;
	dMatrix localMatrix (matrix0 * matrix1.Inverse());
	localMatrix.GetEulerAngles(euler0, euler1);

	AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
	AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
	if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
		euler0 = euler1;
	}
	dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
	for (int i = 0; i < 3; i ++) {
		dFloat error = m_targetAngles[i] - euler[i];
		if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) {
			dFloat angularStep = dSign(error) * m_angulaSpeed * timestep;
			if (angularStep > 0.0f) {
				if (angularStep > error) {
					angularStep = error * 0.5f;
				}
			} else {
				if (angularStep < error) {
					angularStep = error * 0.5f;
				}
			}
			euler[i] = euler[i] + angularStep;
		}
	}
	
	dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
	dMatrix baseMatrix (p0y0r0 * matrix1);
	dMatrix rotation (matrix0.Inverse() * baseMatrix);

	dQuaternion quat (rotation);
	if (quat.m_q0 > dFloat (0.99995f)) {
		dVector euler0;
		dVector euler1;
		rotation.GetEulerAngles(euler0, euler1);
		NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
		NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
		NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
	} else {
		dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
		NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]);
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]); 
		NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]); 
	}
#else

	matrix1 = m_targetRotation * matrix1;

	dQuaternion localRotation(matrix1 * matrix0.Inverse());
	if (localRotation.DotProduct(m_targetRotation) < 0.0f) {
		localRotation.Scale(-1.0f);
	}

	dFloat angle = 2.0f * dAcos(localRotation.m_q0);
	dFloat angleStep = m_angulaSpeed * timestep;
	if (angleStep < angle) {
		dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f));
		axis = axis.Scale(1.0f / dSqrt(axis % axis));
//		localRotation = dQuaternion(axis, angleStep);
	}

	dVector axis (matrix1.m_front * matrix1.m_front);
	dVector axis1 (matrix1.m_front * matrix1.m_front);
//dFloat sinAngle;
//dFloat cosAngle;
//CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle);
//float xxxx = dAtan2(sinAngle, cosAngle);
//float xxxx1 = dAtan2(sinAngle, cosAngle);

	dQuaternion quat(localRotation);
	if (quat.m_q0 > dFloat(0.99995f)) {
//		dAssert (0);
/*
		dVector euler0;
		dVector euler1;
		rotation.GetEulerAngles(euler0, euler1);
		NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]);
		NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]);
		NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]);
*/
	} else {
		dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
		NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]);
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]);
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]);
	}

#endif
}
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]);
		}
	}
}
Esempio n. 23
0
void dComplemtaritySolver::CalculateReactionsForces (int bodyCount, dBodyState** const bodyArray, int jointCount, dBilateralJoint** const jointArray, dFloat timestepSrc, dJacobianPair* const jacobianArray, dJacobianColum* const jacobianColumnArray)
{
	dJacobian stateVeloc[COMPLEMENTARITY_STACK_ENTRIES];
	dJacobian internalForces [COMPLEMENTARITY_STACK_ENTRIES];

	int stateIndex = 0;
	dVector zero(dFloat (0.0f), dFloat (0.0f), dFloat (0.0f), dFloat (0.0f));
	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		stateVeloc[stateIndex].m_linear = state->m_veloc;
		stateVeloc[stateIndex].m_angular = state->m_omega;

		internalForces[stateIndex].m_linear = zero;
		internalForces[stateIndex].m_angular = zero;

		state->m_myIndex = stateIndex;
		stateIndex ++;
		dAssert (stateIndex < int (sizeof (stateVeloc)/sizeof (stateVeloc[0])));
	}

	for (int i = 0; i < jointCount; i ++) {
		dJacobian y0;
		dJacobian y1;
		y0.m_linear = zero;
		y0.m_angular = zero;
		y1.m_linear = zero;
		y1.m_angular = zero;
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			dJacobianPair* const row = &jacobianArray[j + first];
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			y0.m_linear += row->m_jacobian_IM0.m_linear.Scale(val);
			y0.m_angular += row->m_jacobian_IM0.m_angular.Scale(val);
			y1.m_linear += row->m_jacobian_IM1.m_linear.Scale(val);
			y1.m_angular += row->m_jacobian_IM1.m_angular.Scale(val);
		}
		int m0 = constraint->m_state0->m_myIndex;
		int m1 = constraint->m_state1->m_myIndex;
		internalForces[m0].m_linear += y0.m_linear;
		internalForces[m0].m_angular += y0.m_angular;
		internalForces[m1].m_linear += y1.m_linear;
		internalForces[m1].m_angular += y1.m_angular;
	}


	dFloat invTimestepSrc = dFloat (1.0f) / timestepSrc;
	dFloat invStep = dFloat (0.25f);
	dFloat timestep = timestepSrc * invStep;
	dFloat invTimestep = invTimestepSrc * dFloat (4.0f);

	int maxPasses = 5;
	dFloat firstPassCoef = dFloat (0.0f);
	dFloat maxAccNorm = dFloat (1.0e-2f);

	for (int step = 0; step < 4; step ++) {
		dJointAccelerationDecriptor joindDesc;
		joindDesc.m_timeStep = timestep;
		joindDesc.m_invTimeStep = invTimestep;
		joindDesc.m_firstPassCoefFlag = firstPassCoef;

		for (int i = 0; i < jointCount; i ++) {
			dBilateralJoint* const constraint = jointArray[i];
			joindDesc.m_rowsCount = constraint->m_count;
			joindDesc.m_rowMatrix = &jacobianArray[constraint->m_start];
			joindDesc.m_colMatrix = &jacobianColumnArray[constraint->m_start];
			constraint->JointAccelerations (&joindDesc);
		}
		firstPassCoef = dFloat (1.0f);

		dFloat accNorm = dFloat (1.0e10f);
		for (int passes = 0; (passes < maxPasses) && (accNorm > maxAccNorm); passes ++) {
			accNorm = dFloat (0.0f);
			for (int i = 0; i < jointCount; i ++) {

				dBilateralJoint* const constraint = jointArray[i];
				int index = constraint->m_start;
				int rowsCount = constraint->m_count;
				int m0 = constraint->m_state0->m_myIndex;
				int m1 = constraint->m_state1->m_myIndex;

				dVector linearM0 (internalForces[m0].m_linear);
				dVector angularM0 (internalForces[m0].m_angular);
				dVector linearM1 (internalForces[m1].m_linear);
				dVector angularM1 (internalForces[m1].m_angular);

				dBodyState* const state0 = constraint->m_state0;
				dBodyState* const state1 = constraint->m_state1;
				const dMatrix& invInertia0 = state0->m_invInertia;
				const dMatrix& invInertia1 = state1->m_invInertia;
				dFloat invMass0 = state0->m_invMass;
				dFloat invMass1 = state1->m_invMass;

				for (int k = 0; k < rowsCount; k ++) {
					dJacobianPair* const row = &jacobianArray[index];
					dJacobianColum* const col = &jacobianColumnArray[index];

					dVector JMinvIM0linear (row->m_jacobian_IM0.m_linear.Scale (invMass0));
					dVector JMinvIM1linear (row->m_jacobian_IM1.m_linear.Scale (invMass1));
					dVector JMinvIM0angular = invInertia0.UnrotateVector(row->m_jacobian_IM0.m_angular);
					dVector JMinvIM1angular = invInertia1.UnrotateVector(row->m_jacobian_IM1.m_angular);
					dVector acc (JMinvIM0linear.CompProduct(linearM0) + JMinvIM0angular.CompProduct(angularM0) + JMinvIM1linear.CompProduct(linearM1) + JMinvIM1angular.CompProduct(angularM1));

					dFloat a = col->m_coordenateAccel - acc.m_x - acc.m_y - acc.m_z - col->m_force * col->m_diagDamp;
					dFloat f = col->m_force + col->m_invDJMinvJt * a;

					dFloat lowerFrictionForce = col->m_jointLowFriction;
					dFloat upperFrictionForce = col->m_jointHighFriction;

					if (f > upperFrictionForce) {
						a = dFloat (0.0f);
						f = upperFrictionForce;
					} else if (f < lowerFrictionForce) {
						a = dFloat (0.0f);
						f = lowerFrictionForce;
					}

					accNorm = dMax (accNorm, dAbs (a));
					dFloat prevValue = f - col->m_force;
					col->m_force = f;

					linearM0 += row->m_jacobian_IM0.m_linear.Scale (prevValue);
					angularM0 += row->m_jacobian_IM0.m_angular.Scale (prevValue);
					linearM1 += row->m_jacobian_IM1.m_linear.Scale (prevValue);
					angularM1 += row->m_jacobian_IM1.m_angular.Scale (prevValue);
					index ++;
				}
				internalForces[m0].m_linear = linearM0;
				internalForces[m0].m_angular = angularM0;
				internalForces[m1].m_linear = linearM1;
				internalForces[m1].m_angular = angularM1;
			}
		}

		for (int i = 0; i < bodyCount; i ++) {
			dBodyState* const state = bodyArray[i];
			//int index = state->m_myIndex;
			dAssert (state->m_myIndex == i);
			dVector force (state->m_externalForce + internalForces[i].m_linear);
			dVector torque (state->m_externalTorque + internalForces[i].m_angular);
			state->IntegrateForce(timestep, force, torque);
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		int first = constraint->m_start;
		int count = constraint->m_count;
		for (int j = 0; j < count; j ++) { 
			const dJacobianColum* const col = &jacobianColumnArray[j + first];
			dFloat val = col->m_force; 
			constraint->m_jointFeebackForce[j] = val;
		}
	}

	for (int i = 0; i < jointCount; i ++) {
		dBilateralJoint* const constraint = jointArray[i];
		constraint->UpdateSolverForces (jacobianArray);
	}

	for (int i = 0; i < bodyCount; i ++) {
		dBodyState* const state = bodyArray[i];
		dAssert (state->m_myIndex == i);
		state->ApplyNetForceAndTorque (invTimestepSrc, stateVeloc[i].m_linear, stateVeloc[i].m_angular);
	}
}
void dCustomBallAndSocket::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);
	SubmitLinearRows(0x07, matrix0, matrix1);

	const dVector& coneDir0 = matrix0.m_front;
	const dVector& coneDir1 = matrix1.m_front;

	dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0);
	dMatrix coneRotation(dGetIdentityMatrix());
	dVector lateralDir(matrix0.m_up);

	if (cosAngleCos < 0.9999f) {
		lateralDir = coneDir1.CrossProduct(coneDir0);
		dFloat mag2 = lateralDir.DotProduct3(lateralDir);
		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);
		}
	}

	dVector omega0(0.0f);
	dVector omega1(0.0f);
	NewtonBodyGetOmega(m_body0, &omega0[0]);
	if (m_body1) {
		NewtonBodyGetOmega(m_body1, &omega1[0]);
	}
	dVector relOmega(omega0 - omega1);

	// do twist angle calculations
	dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse());
	dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1]));
	if (m_options.m_option0) {
		if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) {
			NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		} else {
			if (m_options.m_option1) {
				// TODO spring option
				dAssert (0);
			} else {
				SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep);
			}
		}
	} else if (m_options.m_option1) {
		// TODO spring option
		dAssert (0);
	} else if (m_twistFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]);
		NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction);
	}

	// do twist cone angle calculations
	if (m_options.m_option2) {
		if ((m_maxConeAngle == 0.0f)) {
			dMatrix localMatrix(matrix0 * matrix1.Inverse());
			dVector euler0;
			dVector euler1;
			localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw);
			NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
			NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]);
			NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
		} else {
			if (m_options.m_option3) {
				// TODO spring option
				dAssert(0);
			} else {
				dFloat jointOmega = relOmega.DotProduct3(lateralDir);
				dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)));
				dFloat coneAngle = currentAngle + jointOmega * timestep;
				if (coneAngle >= m_maxConeAngle) {
					//dQuaternion rot(lateralDir, coneAngle);
					//dVector frontDir(rot.RotateVector(coneDir1));
					//dVector upDir(lateralDir.CrossProduct(frontDir));

					dVector upDir(lateralDir.CrossProduct(coneDir0));
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowStiffness(m_joint, m_stiffness);

					NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
					NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
					const dFloat invtimestep = 1.0f / timestep;
					const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep;
					const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep;
					NewtonUserJointSetRowAcceleration(m_joint, stopAccel);

				} else if (m_coneFriction != 0) {
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);

					dVector upDir(lateralDir.CrossProduct(coneDir0));
					NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
					NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
					NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
					NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
				}
			}
		}
	} else if (m_options.m_option3) {
		// TODO spring option
		dAssert(0);
	} else if (m_coneFriction > 0.0f) {
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);

		dVector upDir(lateralDir.CrossProduct(coneDir0));
		NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
		NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint));
		NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction);
		NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction);
	}
}
void CustomPlayerController::Init(dFloat mass, dFloat outerRadius, dFloat innerRadius, dFloat height, dFloat stairStep, const dMatrix& localAxis)
{
	dAssert (stairStep >= 0.0f);
	dAssert (innerRadius >= 0.0f);
	dAssert (outerRadius >= innerRadius);
	dAssert (height >= stairStep);
	dAssert (localAxis[0].m_w == dFloat (0.0f));
	dAssert (localAxis[1].m_w == dFloat (0.0f));

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

	SetRestrainingDistance (0.0f);

	m_outerRadio = outerRadius;
	m_innerRadio = innerRadius;
	m_height = height;
	m_stairStep = stairStep;
	SetClimbSlope(45.0f * 3.1416f/ 180.0f);
	m_upVector = localAxis[0];
	m_frontVector = localAxis[1];

	m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f);
	m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f);

	const int steps = 12;
	dVector convexPoints[2][steps];

	// create an inner thin cylinder
	dFloat shapeHigh = height;
	dAssert (shapeHigh > 0.0f);
	dVector p0 (0.0f, m_innerRadio, 0.0f, 0.0f);
	dVector p1 (shapeHigh, m_innerRadio, 0.0f, 0.0f);
	for (int i = 0; i < steps; i ++) {
		dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
		convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(p0));
		convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(p1));
	}
	NewtonCollision* const supportShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL); 

	// create the outer thick cylinder
	dMatrix outerShapeMatrix (localAxis);
	dFloat capsuleHigh = m_height - stairStep;
	dAssert (capsuleHigh > 0.0f);
	m_sphereCastOrigin = capsuleHigh * 0.5f + stairStep;
	outerShapeMatrix.m_posit = outerShapeMatrix[0].Scale(m_sphereCastOrigin);
	outerShapeMatrix.m_posit.m_w = 1.0f;
	NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, 0.25f, 0.5f, 0, &outerShapeMatrix[0][0]);
	NewtonCollisionSetScale(bodyCapsule, capsuleHigh, m_outerRadio * 4.0f, m_outerRadio * 4.0f);

	// compound collision player controller
	NewtonCollision* const playerShape = NewtonCreateCompoundCollision(world, 0);
	NewtonCompoundCollisionBeginAddRemove(playerShape);	
	NewtonCompoundCollisionAddSubCollision (playerShape, supportShape);
	NewtonCompoundCollisionAddSubCollision (playerShape, bodyCapsule);
	NewtonCompoundCollisionEndAddRemove (playerShape);	

	// create the kinematic body
	dMatrix locationMatrix (dGetIdentityMatrix());
	m_body = NewtonCreateKinematicBody(world, playerShape, &locationMatrix[0][0]);

	// players must have weight, otherwise they are infinitely strong when they collide
	NewtonCollision* const shape = NewtonBodyGetCollision(m_body);
	NewtonBodySetMassProperties(m_body, mass, shape);

	// make the body collidable with other dynamics bodies, by default
	NewtonBodySetCollidable (m_body, true);

	dFloat castHigh = capsuleHigh * 0.4f;
	dFloat castRadio = (m_innerRadio * 0.5f > 0.05f) ? m_innerRadio * 0.5f : 0.05f;

	dVector q0 (0.0f, castRadio, 0.0f, 0.0f);
	dVector q1 (castHigh, castRadio, 0.0f, 0.0f);
	for (int i = 0; i < steps; i ++) {
		dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
		convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(q0));
		convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(q1));
	}
	m_castingShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL); 


	m_supportShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 0));
	m_upperBodyShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 1));

	NewtonDestroyCollision (bodyCapsule);
	NewtonDestroyCollision (supportShape);
	NewtonDestroyCollision (playerShape);

	m_isJumping = false;
}
Esempio n. 26
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

}
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);
//	}
}
void CustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex)
{
	dVector omega0;
	dVector veloc1;
	dMatrix matrix0;
	dMatrix matrix1;
	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]);

	// get angular velocity relative to the pin vector
	dFloat w0 = omega0 % 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

	// check if this is an impulsive time step
	dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;
	dFloat relAccel = - 0.3f * relVeloc * invTimestep;

	// set the linear part of Jacobian 0 to zero	
	jacobian0[0] = 0.0f;
	jacobian0[1] = 0.0f;	 
	jacobian0[2] = 0.0f;

	// set the angular part of Jacobian 0 pin vector		
	jacobian0[3] = matrix0.m_front[0];
	jacobian0[4] = matrix0.m_front[1];
	jacobian0[5] = matrix0.m_front[2];

	// 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);
}
Esempio n. 29
0
//dVector dMatrix::GetEulerAngles (dEulerAngleOrder order) const
void dMatrix::GetEulerAngles (dVector & euler0, dVector & euler1, dEulerAngleOrder order) const
{
   int a0 = (order >> 8) & 3;
   int a1 = (order >> 4) & 3;
   int a2 = (order >> 0) & 3;
   const dMatrix & matrix = *this;
   // Assuming the angles are in radians.
   if (matrix[a0][a2] > 0.99995f)
   {
      dFloat picth0 = 0.0f;
      dFloat yaw0 = -3.141592f * 0.5f;
      dFloat roll0 = - dAtan2 (matrix[a2][a1], matrix[a1][a1]);
      euler0[a0] = picth0;
      euler0[a1] = yaw0;
      euler0[a2] = roll0;
      euler1[a0] = picth0;
      euler1[a1] = yaw0;
      euler1[a2] = roll0;
   }
   else
      if (matrix[a0][a2] < -0.99995f)
      {
         dFloat picth0 = 0.0f;
         dFloat yaw0 = 3.141592f * 0.5f;
         dFloat roll0 = dAtan2 (matrix[a2][a1], matrix[a1][a1]);
         euler0[a0] = picth0;
         euler0[a1] = yaw0;
         euler0[a2] = roll0;
         euler1[a0] = picth0;
         euler1[a1] = yaw0;
         euler1[a2] = roll0;
      }
      else
      {
         //euler[a0] = -dAtan2(-matrix[a1][a2], matrix[a2][a2]);
         //euler[a1] = -dAsin ( matrix[a0][a2]);
         //euler[a2] = -dAtan2(-matrix[a0][a1], matrix[a0][a0]);
         dFloat yaw0 = -dAsin ( matrix[a0][a2]);
         dFloat yaw1 = 3.141592f - yaw0;
         dFloat sign0 = dSign (dCos (yaw0));
         dFloat sign1 = dSign (dCos (yaw1));
         dFloat picth0 = dAtan2 (matrix[a1][a2] * sign0, matrix[a2][a2] * sign0);
         dFloat picth1 = dAtan2 (matrix[a1][a2] * sign1, matrix[a2][a2] * sign1);
         dFloat roll0 = dAtan2 (matrix[a0][a1] * sign0, matrix[a0][a0] * sign0);
         dFloat roll1 = dAtan2 (matrix[a0][a1] * sign1, matrix[a0][a0] * sign1);
         if (yaw1 > 3.141592f)
            yaw1 -= 2.0f * 3.141592f;
         euler0[a0] = picth0;
         euler0[a1] = yaw0;
         euler0[a2] = roll0;
         euler1[a0] = picth1;
         euler1[a1] = yaw1;
         euler1[a2] = roll1;
      }
   euler0[3] = dFloat (0.0f);
   euler1[3] = dFloat (0.0f);
#ifdef _DEBUG
   if (order == m_pitchYawRoll)
   {
      dMatrix m0 (dPitchMatrix (euler0[0]) * dYawMatrix (euler0[1]) * dRollMatrix (euler0[2]));
      dMatrix m1 (dPitchMatrix (euler1[0]) * dYawMatrix (euler1[1]) * dRollMatrix (euler1[2]));
      for (int i = 0; i < 3; i ++)
      {
         for (int j = 0; j < 3; j ++)
         {
            dFloat error = dAbs (m0[i][j] - matrix[i][j]);
            dAssert (error < 5.0e-2f);
            error = dAbs (m1[i][j] - matrix[i][j]);
            dAssert (error < 5.0e-2f);
         }
      }
   }
#endif
}
Esempio n. 30
0
void AdvanceSimulation (int timeInMilisecunds)
{
	// do the physics simulation here
	int deltaTime;
	int physicLoopsTimeAcc;
	dFloat fps;
	dFloat physicTime;


	// get the time step
	deltaTime = timeInMilisecunds - g_currentTime;
	g_currentTime = timeInMilisecunds;
	g_timeAccumulator += deltaTime;

	physicTime = 0;
	// advance the simulation at a fix step
	int loops = 0;
	physicLoopsTimeAcc = 0;


	while ((loops < MAX_PHYSICS_LOOPS) && (g_timeAccumulator >= DEMO_FPS_IN_MICROSECUNDS))
	{
		loops ++;

		// Process incoming events. 
		ProcessEvents (g_world);

		// sample time before the Update
		g_physicTime = GetTimeInMicrosenconds ();

		// run the newton update function
		NewtonUpdate (g_world, (1.0f / DEMO_PHYSICS_FPS));

		// calculate the time spent in the physical Simulation
		g_physicTime = GetTimeInMicrosenconds () - g_physicTime;

		// call the visual debugger to show the physics scene
#ifdef USE_VISUAL_DEBUGGER
		NewtonDebuggerServe (g_newtonDebugger, g_world);
#endif

		// subtract time from time accumulator
		g_timeAccumulator -= DEMO_FPS_IN_MICROSECUNDS;
		physicTime ++;

		physicLoopsTimeAcc += g_physicTime;
	}

	if (loops > MAX_PHYSICS_LOOPS) {
		g_physicTime = physicLoopsTimeAcc;
		g_timeAccumulator = DEMO_FPS_IN_MICROSECUNDS;
	}


	// Clear the color and depth buffers. 
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	// calculate the interpolation parameter for smooth rendering 
	g_sceneManager->SetIntepolationParam(dFloat (g_timeAccumulator) / dFloat(DEMO_FPS_IN_MICROSECUNDS));

	// do the rendering here entire Scene
	g_sceneManager->Render ();

	// display the frame rate
	// smooth the fps by averaging the last few rendering time steps;
	int dtAcc = 0;
	static int fpsIndex;
	static long int smoothFPS[128];

	smoothFPS[fpsIndex] = deltaTime;
	fpsIndex = (fpsIndex + 1) % (sizeof (smoothFPS) / sizeof (smoothFPS[0]));
	for (int i = 0; i < (sizeof (smoothFPS) / sizeof (smoothFPS[0])); i ++) {
		dtAcc += smoothFPS[i];
	}
	dtAcc /= (sizeof (smoothFPS) / sizeof (smoothFPS[0]));

	fps = 1000000.0f / dFloat (dtAcc);
	physicTime = g_physicTime * dFloat(1000.0f / 1000000.0f);
	Print (dVector (1.0f, 1.0f, 0.0f, 0.0f), 10, 10, "fps: %6.2f", fps);
	Print (dVector (1.0f, 1.0f, 0.0f, 0.0f), 10, 22, "physic time (milliseconds): %5.2f", physicTime);

	// show GL rendered Scene 
	glFlush();
	SDL_GL_SwapBuffers( );
}