CustomVehicleControllerBodyStateTire* AddTire (const dVector& offset, dFloat width, dFloat radius, dFloat mass, dFloat suspensionLength, dFloat suspensionSpring, dFloat suspensionDamper, dFloat lateralStiffness, dFloat longitudinalStiffness, dFloat aligningMomentTrail, const dMatrix& tireAligmentMatrix) { NewtonBody* const body = m_controller->GetBody(); // make the tire matrix from the offset and the body matrix dMatrix tireMatrix (GetNextMatrix()); tireMatrix.m_posit = offset; // add the visual representation of the is tire to as a child of the vehicle model NewtonCollision* const tireMeshGenerator = NewtonCreateChamferCylinder (NewtonBodyGetWorld(body), 0.5f, 1.0f, 0, NULL); NewtonCollisionSetScale (tireMeshGenerator, width, radius, radius); DemoEntity* const tireEntity = new DemoEntity (tireMatrix, this); DemoMesh* const visualMesh = new DemoMesh ("tireMesh", tireMeshGenerator, "smilli.tga", "smilli.tga", "smilli.tga"); tireEntity->SetMesh (visualMesh, dYawMatrix(3.141592f * 90.0f / 180.0f)); visualMesh->Release(); NewtonDestroyCollision (tireMeshGenerator); // add the tire to the vehicle CustomVehicleControllerBodyStateTire::TireCreationInfo tireInfo; tireInfo.m_location = tireMatrix.m_posit; tireInfo.m_mass = mass; tireInfo.m_radio = radius; tireInfo.m_width = width; tireInfo.m_dampingRatio = suspensionDamper; tireInfo.m_springStrength = suspensionSpring; tireInfo.m_suspesionlenght = suspensionLength; tireInfo.m_lateralStiffness = lateralStiffness; tireInfo.m_longitudialStiffness = longitudinalStiffness; tireInfo.m_aligningMomentTrail = aligningMomentTrail; tireInfo.m_userData = tireEntity; return m_controller->AddTire (tireInfo); }
void dNewtonCollision::SetScale(dFloat scaleX, dFloat scaleY, dFloat scaleZ) { scaleX = dMax(0.01f, dAbs(scaleX)); scaleY = dMax(0.01f, dAbs(scaleY)); scaleZ = dMax(0.01f, dAbs(scaleZ)); NewtonCollisionSetScale(m_shape, scaleX, scaleY, scaleZ); }
static void AddUniformScaledPrimitives (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix) { // create the shape and visual mesh as a common data to be re used NewtonWorld* const world = scene->GetNewton(); NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID); dFloat startElevation = 1000.0f; dMatrix matrix (dRollMatrix(-3.141592f/2.0f)); for (int i = 0; i < xCount; i ++) { dFloat x = origin.m_x + (i - xCount / 2) * spacing; for (int j = 0; j < zCount; j ++) { dFloat scale = 0.75f + 1.0f * (dFloat (dRand()) / dFloat(dRAND_MAX) - 0.5f); //scale = 1.0f; NewtonCollisionSetScale (collision, scale, scale, scale); DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga"); dFloat z = origin.m_z + (j - zCount / 2) * spacing; matrix.m_posit.m_x = x; matrix.m_posit.m_z = z; dVector floor (FindFloor (world, dVector (matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation)); matrix.m_posit.m_y = floor.m_y + 4.f; // create a solid CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID); // release the mesh geometry->Release(); } } // do not forget to delete the collision NewtonDestroyCollision (collision); }
static void AddNonUniformScaledPrimitives(DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix) { // create the shape and visual mesh as a common data to be re used NewtonWorld* const world = scene->GetNewton(); // NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID); NewtonCollision* const collision = CreateConvexCollision(world, dGetIdentityMatrix(), size, type, materialID); dFloat startElevation = 1000.0f; dMatrix matrix(dGetIdentityMatrix()); //matrix = dPitchMatrix(-45.0f * 3.141592f/180.0f); for (int i = 0; i < xCount; i++) { dFloat x = origin.m_x + (i - xCount / 2) * spacing; for (int j = 0; j < zCount; j++) { dFloat scalex = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f); dFloat scaley = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f); dFloat scalez = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f); dFloat z = origin.m_z + (j - zCount / 2) * spacing; matrix.m_posit.m_x = x; matrix.m_posit.m_z = z; dVector floor(FindFloor(world, dVector(matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation)); matrix.m_posit.m_y = floor.m_y + 8.0f; // create a solid //NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID); NewtonBody* const body = CreateSimpleSolid(scene, NULL, mass, matrix, collision, materialID); DemoEntity* entity = (DemoEntity*)NewtonBodyGetUserData(body); NewtonCollisionSetScale(collision, scalex, scaley, scalez); DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga"); entity->SetMesh(geometry, dGetIdentityMatrix()); NewtonBodySetCollisionScale(body, scalex, scaley, scalez); dVector omega(0.0f); NewtonBodySetOmega(body, &omega[0]); // release the mesh geometry->Release(); } } // do not forget to delete the collision NewtonDestroyCollision(collision); }
static void AddSingleCompound(DemoEntityManager* const scene) { NewtonWorld* const world = scene->GetNewton(); NewtonCollision* compoundCollision = NewtonCreateCompoundCollision(world, 0); NewtonCompoundCollisionBeginAddRemove(compoundCollision); NewtonCollision* boxCollision = NewtonCreateBox(world, 50, 50, 50, 0, NULL); NewtonCompoundCollisionAddSubCollision(compoundCollision, boxCollision); NewtonDestroyCollision(boxCollision); dMatrix matrix(dGetIdentityMatrix()); matrix.m_posit.m_y = 10.0f; NewtonCompoundCollisionEndAddRemove(compoundCollision); NewtonBody* compoundBody = NewtonCreateDynamicBody(world, compoundCollision, &matrix[0][0]); NewtonDestroyCollision(compoundCollision); // scale after creating body slows everything down. Without the scale it runs fine even though the body is huge NewtonCollisionSetScale(NewtonBodyGetCollision(compoundBody), 0.05f, 0.05f, 0.05f); // adding some visualization NewtonBodySetMassProperties (compoundBody, 1.0f, NewtonBodyGetCollision(compoundBody)); NewtonBodySetTransformCallback(compoundBody, DemoEntity::TransformCallback); NewtonBodySetForceAndTorqueCallback(compoundBody, PhysicsApplyGravityForce); DemoMesh* mesh = new DemoMesh("geometry", NewtonBodyGetCollision(compoundBody), "smilli.tga", "smilli.tga", "smilli.tga"); DemoEntity* const entity = new DemoEntity(matrix, NULL); entity->SetMesh(mesh, dGetIdentityMatrix()); mesh->Release(); NewtonBodySetUserData(compoundBody, entity); scene->Append(entity); NewtonBodySetSimulationState(compoundBody, 0); NewtonBodySetSimulationState(compoundBody, 1); }
dCustomPlayerController* dCustomPlayerControllerManager::CreateController(const dMatrix& location, const dMatrix& localAxis, dFloat mass, dFloat radius, dFloat height, dFloat stepHeight) { NewtonWorld* const world = GetWorld(); dMatrix shapeMatrix(localAxis); shapeMatrix.m_posit = shapeMatrix.m_front.Scale (height * 0.5f); shapeMatrix.m_posit.m_w = 1.0f; dFloat scale = 3.0f; height = dMax(height - 2.0f * radius / scale, dFloat(0.1f)); NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, radius / scale, radius / scale, height, 0, &shapeMatrix[0][0]); NewtonCollisionSetScale(bodyCapsule, 1.0f, scale, scale); // create the kinematic body NewtonBody* const body = NewtonCreateKinematicBody(world, bodyCapsule, &location[0][0]); // players must have weight, otherwise they are infinitely strong when they collide NewtonCollision* const shape = NewtonBodyGetCollision(body); NewtonBodySetMassProperties(body, mass, shape); // make the body collidable with other dynamics bodies, by default NewtonBodySetCollidable(body, 1); NewtonDestroyCollision(bodyCapsule); dCustomPlayerController& controller = m_playerList.Append()->GetInfo(); controller.m_localFrame = localAxis; controller.m_mass = mass; controller.m_invMass = 1.0f / mass; controller.m_manager = this; controller.m_kinematicBody = body; controller.m_contactPatch = radius / scale; controller.m_stepHeight = dMax (stepHeight, controller.m_contactPatch * 2.0f); return &controller; }
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; }
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex) { dMatrix matrix; dQuaternion bodyRotation; dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); dVector omega(0.0f, 0.0f, 0.0f, 0.0f); CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); // apply the player motion, by calculation the desired plane linear and angular velocity manager->ApplyPlayerMove (this, timestep); // get the body motion state NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetVelocity(m_body, &veloc[0]); NewtonBodyGetOmega(m_body, &omega[0]); // integrate body angular velocity NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); bodyRotation = bodyRotation.IntegrateOmega(omega, timestep); matrix = dMatrix (bodyRotation, matrix.m_posit); // integrate linear velocity dFloat normalizedTimeLeft = 1.0f; dFloat step = timestep * dSqrt (veloc % veloc) ; dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS); int prevContactCount = 0; CustomControllerConvexCastPreFilter castFilterData (m_body); NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector updir (matrix.RotateVector(m_upVector)); dVector scale; NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z); //const dFloat radio = m_outerRadio * 4.0f; const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f; NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio); NewtonWorldConvexCastReturnInfo upConstratint; memset (&upConstratint, 0, sizeof (upConstratint)); upConstratint.m_normal[0] = m_upVector.m_x; upConstratint.m_normal[1] = m_upVector.m_y; upConstratint.m_normal[2] = m_upVector.m_z; upConstratint.m_normal[3] = m_upVector.m_w; for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) { if ((veloc % veloc) < 1.0e-6f) { break; } dFloat timetoImpact; NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector destPosit (matrix.m_posit + veloc.Scale (timestep)); int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex); if (contactCount) { contactCount = manager->ProcessContacts (this, info, contactCount); } if (contactCount) { matrix.m_posit += veloc.Scale (timetoImpact * timestep); if (timetoImpact > 0.0f) { matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; } normalizedTimeLeft -= timetoImpact; dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; for (int i = 1; i < contactCount; i ++) { dVector n0 (info[i-1].m_normal); for (int j = 0; j < i; j ++) { dVector n1 (info[j].m_normal); if ((n0 % n1) > 0.9999f) { info[i] = info[contactCount - 1]; i --; contactCount --; break; } } } int count = 0; if (!m_isJumping) { upConstratint.m_point[0] = matrix.m_posit.m_x; upConstratint.m_point[1] = matrix.m_posit.m_y; upConstratint.m_point[2] = matrix.m_posit.m_z; upConstratint.m_point[3] = matrix.m_posit.m_w; speed[count] = 0.0f; bounceNormal[count] = dVector (upConstratint.m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint); count ++; } for (int i = 0; i < contactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (info[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]); count ++; } for (int i = 0; i < prevContactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (prevInfo[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]); count ++; } dFloat residual = 10.0f; dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) { residual = 0.0f; for (int k = 0; k < count; k ++) { dVector normal (bounceNormal[k]); dFloat v = bounceSpeed[k] - normal % auxBounceVeloc; dFloat x = speed[k] + v; if (x < 0.0f) { v = 0.0f; x = 0.0f; } if (dAbs (v) > residual) { residual = dAbs (v); } auxBounceVeloc += normal.Scale (x - speed[k]); speed[k] = x; } } dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < count; i ++) { dVector normal (bounceNormal[i]); velocStep += normal.Scale (speed[i]); } veloc += velocStep; dFloat velocMag2 = velocStep % velocStep; if (velocMag2 < 1.0e-6f) { dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep); matrix.m_posit += veloc.Scale (advanceTime); normalizedTimeLeft -= advanceTime / timestep; } prevContactCount = contactCount; memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo)); } else { matrix.m_posit = destPosit; matrix.m_posit.m_w = 1.0f; break; } } NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z); // determine if player is standing on some plane dMatrix supportMatrix (matrix); supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin); if (m_isJumping) { dVector dst (matrix.m_posit); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } else { step = dAbs (updir % veloc.Scale (timestep)); dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step; dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f)); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } // set player velocity, position and orientation NewtonBodySetVelocity(m_body, &veloc[0]); NewtonBodySetMatrix (m_body, &matrix[0][0]); }