dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const
{
dBigVector stackPool[32][3];
int stack = 0;
dFloat64 length = 0.0f;
dFloat64 tol2 = tol * tol;
dFloat64 u0 = m_knotVector[m_degree];
dBigVector p0 (CurvePoint (u0));
for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) {
dFloat64 u1 = m_knotVector[i + 1];
dBigVector p1 (CurvePoint (u1));
stackPool[stack][0] = p0;
stackPool[stack][1] = p1;
stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
while (stack) {
stack --;
dBigVector q0 (stackPool[stack][0]);
dBigVector q1 (stackPool[stack][1]);
dFloat64 t0 = stackPool[stack][2][0];
dFloat64 t1 = stackPool[stack][2][1];
dFloat64 t01 = (t1 + t0) * 0.5f;
dBigVector p01 ((q1 + q0).Scale (0.5f));
dBigVector q01 (CurvePoint (t01));
dBigVector err (q01 - p01);
dFloat64 err2 = err.DotProduct3(err);
if (err2 < tol2) {
dBigVector step (q1 - q0);
length += dSqrt (step.DotProduct3(step));
} else {
stackPool[stack][0] = q01;
stackPool[stack][1] = q1;
stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
stackPool[stack][0] = q0;
stackPool[stack][1] = q01;
stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f));
stack ++;
}
}
u0 = u1;
p0 = p1;
}
return length;
}
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 / 3.0f, 2.0f / 3.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(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.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();
NewtonBody* const box0 = CreateWheel(scene, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
NewtonBodyGetMatrix (box0, &matrix[0][0]);
matrix.m_posit.m_y += 0.5f;
new MyPathFollow (matrix, box0, rollerCosterPath);
}
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
}
