static void AddGearAndRack (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const reel0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
dMatrix matrix;
NewtonBodySetMassMatrix(reel0, 0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetMassMatrix(reel1, 0.0f, 0.0f, 0.0f, 0.0f);
CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
CustomCorkScrew* const cylinder = AddCylindricalWheel(scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
cylinder->EnableLinearLimits(true);
cylinder->SetLinearLimis(-2.0f, 2.0f);
NewtonBody* const body0 = hinge0->GetBody0();
NewtonBody* const body1 = hinge1->GetBody0();
NewtonBody* const body2 = cylinder->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
dMatrix matrix2;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
NewtonBodyGetMatrix (body2, &matrix2[0][0]);
dVector pin0 (matrix0.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
dVector pin2 (matrix2.RotateVector(dVector( 1.0f, 0.0f, 0.0f)));
new CustomGear (1.0f, pin0, pin2, body0, body2);
new CustomRackAndPinion (1.0f, pin1, pin2, body1, body2);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
// make an aggregate for disabling collisions
void* const aggregate = NewtonCollisionAggregateCreate(scene->GetNewton());
NewtonCollisionAggregateSetSelfCollision(aggregate, 0);
NewtonCollisionAggregateAddBody(aggregate, reel0);
NewtonCollisionAggregateAddBody(aggregate, reel1);
NewtonCollisionAggregateAddBody(aggregate, body0);
NewtonCollisionAggregateAddBody(aggregate, body1);
NewtonCollisionAggregateAddBody(aggregate, body2);
}
void AddPulley (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const reel0 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 2.0f), dVector(4.0f, 0.25f, 0.25f));
// this is just for show
NewtonBody* const reel1 = CreateBox(scene, origin + dVector (0.0f, 4.0f, 0.0f), dVector(4.0f, 0.25f, 0.25f));
NewtonBodySetMassMatrix (reel0, 0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetMassMatrix (reel1, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix matrix;
CustomSlider* const slider0 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 2.0f), 0.5f, 1.0f, reel0);
CustomSlider* const slider1 = AddSliderWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.5f, 0.5f, reel0);
slider0->EnableLimits(true);
slider0->SetLimits (-2.0f, 2.0f);
NewtonBody* const body0 = slider0->GetBody0();
NewtonBody* const body1 = slider1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomPulley (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
// make an aggregate for disabling collisions
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
NewtonCollisionAggregateSetSelfCollision (aggregate, 0);
NewtonCollisionAggregateAddBody (aggregate, reel0);
NewtonCollisionAggregateAddBody (aggregate, reel1);
NewtonCollisionAggregateAddBody (aggregate, body0);
NewtonCollisionAggregateAddBody (aggregate, body1);
}
CustomArticulatedTransformController::dSkeletonBone* CustomArticulatedTransformController::AddBone (NewtonBody* const bone, const dMatrix& bindMatrix, dSkeletonBone* const parentBone)
{
m_bones[m_boneCount].m_body = bone;
m_bones[m_boneCount].m_myController = this;
m_bones[m_boneCount].m_parent = parentBone;
m_bones[m_boneCount].m_bindMatrix = bindMatrix;
NewtonCollisionAggregateAddBody (m_collisionAggregate, bone);
m_boneCount ++;
dAssert (m_boneCount < D_HIERACHICAL_CONTROLLER_MAX_BONES);
return &m_bones[m_boneCount - 1];
}
static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
{
// create a Bezier Spline path for AI car to drive
DemoEntity* const rollerCosterPath = new DemoEntity(dGetIdentityMatrix(), NULL);
scene->Append(rollerCosterPath);
dBezierSpline spline;
dFloat64 knots[] = {0.0f, 1.0f / 5.0f, 2.0f / 5.0f, 3.0f / 5.0f, 4.0f / 5.0f, 1.0f};
dBigVector o (origin[0], origin[1], origin[2], 0.0f);
dBigVector control[] =
{
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f) + o,
dBigVector(175.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(215.0f - 100.0f, 20.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 50.0f, 350.0f - 250.0f, 1.0f) + o,
dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
};
spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);
DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
mesh->SetVisible(true);
mesh->SetRenderResolution(500);
mesh->Release();
const int count = 32;
NewtonBody* bodies[count];
dBigVector point0;
dVector positions[count + 1];
dFloat64 knot = spline.FindClosestKnot(point0, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 4);
positions[0] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
dFloat average = 0.0f;
for (int i = 0; i < count; i ++) {
dBigVector point1;
average += positions[i].m_y;
dBigVector tangent(spline.CurveDerivative(knot));
tangent = tangent.Scale (1.0 / sqrt (tangent % tangent));
knot = spline.FindClosestKnot(point1, dBigVector (point0 + tangent.Scale (2.0f)), 4);
point0 = point1;
positions[i + 1] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
}
average /= count;
for (int i = 0; i < count + 1; i ++) {
positions[i].m_y = average;
}
dFloat attachmentOffset = 0.8f;
for (int i = 0; i < count; i ++) {
dMatrix matrix;
dVector location0 (positions[i].m_x, positions[i].m_y, positions[i].m_z, 0.0);
bodies[i] = CreateWheel(scene, location0, 1.0f, 0.5f);
NewtonBodySetLinearDamping(bodies[i], 0.0f);
NewtonBody* const box = bodies[i];
NewtonBodyGetMatrix(box, &matrix[0][0]);
dVector location1 (positions[i + 1].m_x, positions[i + 1].m_y, positions[i + 1].m_z, 0.0);
dVector dir (location1 - location0);
matrix.m_front = dir.Scale (1.0f / dSqrt (dir % dir));
matrix.m_right = matrix.m_front * matrix.m_up;
dMatrix matrix1 (dYawMatrix(0.5f * 3.141692f) * matrix);
NewtonBodySetMatrix(box, &matrix1[0][0]);
matrix.m_posit.m_y += attachmentOffset;
new MyPathFollow(matrix, box, rollerCosterPath);
dVector veloc (dir.Scale (20.0f));
NewtonBodySetVelocity(box, &veloc[0]);
}
for (int i = 1; i < count; i ++) {
NewtonBody* const box0 = bodies[i - 1];
NewtonBody* const box1 = bodies[i];
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix(box0, &matrix0[0][0]);
NewtonBodyGetMatrix(box1, &matrix1[0][0]);
matrix0.m_posit.m_y += attachmentOffset;
matrix1.m_posit.m_y += attachmentOffset;
new CustomDistanceRope (matrix1.m_posit, matrix0.m_posit, box1, box0);
}
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
for (int i = 0; i < count; i ++) {
NewtonCollisionAggregateAddBody(aggregate, bodies[i]);
}
NewtonCollisionAggregateSetSelfCollision (aggregate, false);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), bodies[0], NULL);
for (int i = 1; i < count; i++) {
NewtonSkeletonContainerAttachBone(skeleton, bodies[i], bodies[i - 1]);
}
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
