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 }