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 ++; }
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)); }
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; }
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); }
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; }
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())); }
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); }
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; }
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]); } } }
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; }
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); }
//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 }
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( ); }