static void AddBallAndSockectWithFriction (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocketWithFriction (pinMatrix, box0, NULL, 20.0f);
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, & matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
new CustomBallAndSocketWithFriction (pinMatrix, box1, box0, 10.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void Add6DOF (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
const dFloat angle = 60.0f * 3.1415592f / 180.0f;
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, & matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
Custom6DOF* const joint0 = new Custom6DOF (pinMatrix, pinMatrix, box0, NULL);
joint0->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
// link the two boxes
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
Custom6DOF* const joint1 = new Custom6DOF (pinMatrix, pinMatrix, box0, box1);
joint1->SetAngularLimits (dVector (-angle, -angle, -angle, 0.0f), dVector (angle, angle, angle, 0.0f));
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddCylindrical (DemoEntityManager* const scene, const dVector& origin)
{
// make a reel static
NewtonBody* const box0 = CreateCylinder (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 0.25f, 8.0f);
NewtonBody* const box1 = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
//NewtonBodyGetMatrix(box0, &matrix[0][0]);
//CustomHinge* const fixJoint = new CustomHinge(matrix, box0, NULL);
//fixJoint->EnableLimits(true);
//fixJoint->SetLimis(0.0f, 0.0f);
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect the bodies by a CorkScrew joint
NewtonBodyGetMatrix (box1, &matrix[0][0]);
CustomCorkScrew* const cylinder = new CustomCorkScrew (matrix, box1, box0);
// enable limit of first axis
cylinder->EnableLinearLimits(true);
// set limit on second axis
cylinder->SetLinearLimis (-4.0f, 4.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddGear (DemoEntityManager* const scene, const dVector& origin)
{
NewtonBody* const box0 = CreateCylinder(scene, origin + dVector (0.0f, 4.0f, 0.0f), 0.25f, 4.0f);
// this is a fix joint
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect two bodies with a hinge
CustomHinge* const hinge0 = AddHingeWheel (scene, origin + dVector (-1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
CustomHinge* const hinge1 = AddHingeWheel (scene, origin + dVector ( 1.0f, 4.0f, 0.0f), 0.5f, 1.0f, box0);
NewtonBody* const body0 = hinge0->GetBody0();
NewtonBody* const body1 = hinge1->GetBody0();
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix (body0, &matrix0[0][0]);
NewtonBodyGetMatrix (body1, &matrix1[0][0]);
// relate the two body motion with a gear joint
dVector pin0 (matrix0.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
dVector pin1 (matrix1.RotateVector(dVector (1.0f, 0.0f, 0.0f)));
new CustomGear (4.0f, pin0, pin1, body0, body1);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
// NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), box0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddSlider (DemoEntityManager* const scene, const dVector& origin)
{
// make a reel static
NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), dVector (8.0f, 0.25f, 0.25f, 0.0f));
NewtonBody* const box1 = CreateWheel (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// connect the bodies by a Slider joint
NewtonBodyGetMatrix (box1, &matrix[0][0]);
CustomSlider* const slider = new CustomSlider (matrix, box1, box0);
// enable limit of first axis
slider->EnableLimits(true);
// set limit on second axis
slider->SetLimits (-4.0f, 4.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddSlidingContact(DemoEntityManager* const scene, const dVector& origin)
{
// make a reel static
NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(8.0f, 0.25f, 0.25f, 0.0f));
NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), 1.0f, 0.5f);
dMatrix matrix;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
NewtonBodyGetMatrix(box0, &matrix[0][0]);
CustomHinge* const hinge = new CustomHinge(matrix, box0, NULL);
hinge->EnableLimits(true);
hinge->SetLimis(0.0f, 0.0f);
// connect the bodies by a Slider joint
NewtonBodyGetMatrix(box1, &matrix[0][0]);
CustomSlidingContact* const slider = new CustomSlidingContact(matrix, box1, box0);
slider->EnableLinearLimits (true);
slider->SetLinearLimis (-4.0f, 4.0f);
// enable limit of first axis
slider->EnableAngularLimits(true);
slider->SetAngularLimis (-60.0f * 3.1416f / 180.0f, 60.0f * 3.1416f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddDistance (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateCapule (scene, origin + dVector (0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule (scene, origin + dVector (0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
dMatrix pinMatrix (dGrammSchmidt (dVector (0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix0;
NewtonBodyGetMatrix (box0, &matrix0[0][0]);
pinMatrix.m_posit = matrix0.m_posit + dVector (0.0f, size.m_y, 0.0f, 0.0f);
new CustomBallAndSocket (pinMatrix, box0, NULL);
// link the two boxes with a distance joint
dMatrix matrix1;
NewtonBodyGetMatrix (box1, &matrix1[0][0]);
// get the origins
dVector pivot0 (matrix0.m_posit);
dVector pivot1 (matrix1.m_posit);
// connect bodies at a corner
new CustomDistance (pivot1, pivot0, box1, box0);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
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;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
NewtonBodyGetMatrix(reel0, &matrix[0][0]);
CustomHinge* const fixJoint0 = new CustomHinge(matrix, reel0, NULL);
fixJoint0->EnableLimits(true);
fixJoint0->SetLimis(0.0f, 0.0f);
NewtonBodyGetMatrix(reel1, &matrix[0][0]);
CustomHinge* const fixJoint1 = new CustomHinge(matrix, reel1, NULL);
fixJoint1->EnableLimits(true);
fixJoint1->SetLimis(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, reel1);
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(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, hinge0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, hinge1->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, reel1, NULL);
NewtonSkeletonContainerAttachBone(skeleton, cylinder->GetBody0(), reel1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
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 AddHinge (DemoEntityManager* const scene, const dVector& origin)
{
dVector size (1.5f, 4.0f, 0.125f);
NewtonBody* const box0 = CreateBox (scene, origin + dVector (0.0f, 4.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateBox (scene, origin + dVector (1.5f, 4.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateBox (scene, origin + dVector (3.0f, 4.0f, 0.0f, 0.0f), size);
//the joint pin is the first row of the matrix, to make a upright pin we
//take the x axis and rotate by 90 degree around the y axis
dMatrix localPin (dRollMatrix(90.0f * 3.141592f / 180.0f));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix (box0, & matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
// add hinge with limit and friction
CustomHinge* const hinge0 = new CustomHinge (matrix, box0, NULL);
hinge0->EnableLimits (true);
hinge0->SetLimis(-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge0->SetFriction (20.0f);
// link the two boxes
NewtonBodyGetMatrix (box1, &matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
CustomHinge* const hinge1 = new CustomHinge (matrix, box1, box0);
hinge1->EnableLimits (true);
hinge1->SetLimis (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge1->SetFriction (20.0f);
// link the two boxes
NewtonBodyGetMatrix (box2, &matrix[0][0]);
matrix.m_posit += dVector (-size.m_x * 0.5f, 0.0f, 0.0f);
matrix = localPin * matrix;
CustomHinge* const hinge2 = new CustomHinge (matrix, box2, box1);
hinge2->EnableLimits (true);
hinge2->SetLimis (-45.0f * 3.141592f / 180.0f, 45.0f * 3.141592f / 180.0f);
hinge2->SetFriction (20.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), NULL, NULL);
// NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone (skeleton, box0, NULL);
NewtonSkeletonContainerAttachBone (skeleton, box1, box0);
NewtonSkeletonContainerAttachBone (skeleton, box2, box1);
NewtonSkeletonContainerFinalize (skeleton);
#endif
}
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));
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;
//connect the box0 to the base by a fix joint (a hinge with zero limit)
NewtonBodyGetMatrix(reel0, &matrix[0][0]);
CustomHinge* const fixJoint0 = new CustomHinge(matrix, reel0, NULL);
fixJoint0->EnableLimits(true);
fixJoint0->SetLimis(0.0f, 0.0f);
NewtonBodyGetMatrix(reel1, &matrix[0][0]);
CustomHinge* const fixJoint1 = new CustomHinge(matrix, reel1, NULL);
fixJoint1->EnableLimits(true);
fixJoint1->SetLimis(0.0f, 0.0f);
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, reel1);
slider0->EnableLimits(true);
slider0->SetLimis (-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(), NULL, NULL);
NewtonSkeletonContainerAttachBone(skeleton, reel0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, slider0->GetBody0(), reel0);
NewtonSkeletonContainerAttachBone(skeleton, reel1, NULL);
NewtonSkeletonContainerAttachBone(skeleton, slider1->GetBody0(), reel1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddUniversal(DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const box0 = CreateBox(scene, origin + dVector(0.0f, 4.0f, 0.0f, 0.0f), dVector(0.25f, 0.25f, 4.0f, 0.0f));
NewtonBody* const box1 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, 2.0f, 0.0f), 1.0f, 0.5f);
NewtonBody* const box2 = CreateWheel(scene, origin + dVector(0.0f, 4.0f, -2.0f, 0.0f), 1.0f, 0.5f);
NewtonBodySetMassMatrix(box0, 0.0f, 0.0f, 0.0f, 0.0f);
// align the object so that is looks nice
dMatrix matrix;
NewtonBodyGetMatrix(box1, &matrix[0][0]);
matrix = dYawMatrix (3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box1, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box1))->ResetMatrix (*scene, matrix);
NewtonBodyGetMatrix(box2, &matrix[0][0]);
matrix = dYawMatrix(3.1416f * 0.5f) * matrix;
NewtonBodySetMatrix(box2, &matrix[0][0]);
((DemoEntity*) NewtonBodyGetUserData(box2))->ResetMatrix (*scene, matrix);
// link the two boxes
NewtonBodyGetMatrix(box1, &matrix[0][0]);
CustomUniversal* const joint1 = new CustomUniversal(matrix, box1, box0);
joint1->EnableLimit_0(true);
joint1->SetLimis_0 (-5.0f * 3.141592f, 2.0f * 3.141592f);
joint1->EnableLimit_1(true);
joint1->SetLimis_1 (-3.0f * 3.141592f, 4.0f * 3.141592f);
// link the two boxes
NewtonBodyGetMatrix(box2, &matrix[0][0]);
CustomUniversal* const joint2 = new CustomUniversal(matrix, box2, box0);
joint2->EnableLimit_0(true);
joint2->SetLimis_0 (-3.0f * 3.141592f, 5.0f * 3.141592f);
joint2->EnableLimit_1(true);
joint2->SetLimis_1(-4.0f * 3.141592f, 2.0f * 3.141592f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), box0, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box0);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
static void AddLimitedBallAndSocket (DemoEntityManager* const scene, const dVector& origin)
{
dVector size(1.0f, 1.0f, 1.0f);
NewtonBody* const base = CreateBox(scene, origin + dVector (0.0f, 5.0f + size.m_y + 0.25f, 0.0f, 0.0f), size.Scale (0.2f));
NewtonBody* const box0 = CreateCapule(scene, origin + dVector(0.0f, 5.0f, 0.0f, 0.0f), size);
NewtonBody* const box1 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 2.0f, 0.0f, 0.0f), size);
NewtonBody* const box2 = CreateCapule(scene, origin + dVector(0.0f, 5.0 - size.m_y * 4.0f, 0.0f, 0.0f), size);
NewtonBodySetMassMatrix(base, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix pinMatrix(dGrammSchmidt(dVector(0.0f, -1.0f, 0.0f, 0.0f)));
// connect first box to the world
dMatrix matrix;
NewtonBodyGetMatrix(box0, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint0 = new CustomLimitBallAndSocket(pinMatrix, box0, base);
joint0->SetConeAngle (30.0f * 3.141592f / 180.0f);
joint0->SetTwistAngle (-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box1 to box0 the world
NewtonBodyGetMatrix(box1, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint1 = new CustomLimitBallAndSocket(pinMatrix, box1, box0);
joint1->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint1->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
// connect first box2 to box1 the world
NewtonBodyGetMatrix(box2, &matrix[0][0]);
pinMatrix.m_posit = matrix.m_posit + dVector(0.0f, size.m_y, 0.0f, 0.0f);
CustomLimitBallAndSocket* const joint2 = new CustomLimitBallAndSocket(pinMatrix, box2, box1);
joint2->SetConeAngle(30.0f * 3.141592f / 180.0f);
joint2->SetTwistAngle(-30.0f * 3.141592f / 180.0f, 30.0f * 3.141592f / 180.0f);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), base, NULL);
NewtonSkeletonContainerAttachBone(skeleton, box0, base);
NewtonSkeletonContainerAttachBone(skeleton, box1, box0);
NewtonSkeletonContainerAttachBone(skeleton, box2, box1);
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void CustomArticulatedTransformController::MakeNewtonSkeleton() const
{
NewtonBody* bonePool[32];
NewtonBody* boneParent[32];
int stack = 1;
bonePool[0] = GetBoneBody(0);
boneParent[0] = NULL;
NewtonWorld* const world = ((CustomArticulaledTransformManager*)GetManager())->GetWorld();
NewtonSkeletonContainer* skeleton = NULL;
while (stack) {
stack--;
NewtonBody* const bone = bonePool[stack];
NewtonBody* const parent = boneParent[stack];
if (!skeleton) {
skeleton = NewtonSkeletonContainerCreate(world, bone, CustomArticulaledTransformManager::OnControllerDestroy);
} else {
NewtonSkeletonContainerAttachBone(skeleton, bone, parent);
}
for (int i = 0; i < m_boneCount; i++) {
const CustomArticulatedTransformController::dSkeletonBone* const child = GetBone(i);
if (child->m_parent && child->m_parent->m_body == bone) {
boneParent[stack] = bone;
bonePool[stack] = child->m_body;
stack++;
}
}
}
NewtonSkeletonContainerFinalize(skeleton);
/*
for (int i = 0; i < m_boneCount; i++) {
const CustomArticulatedTransformController::dSkeletonBone* const child = GetBone(i);
NewtonCollisionAggregateRemoveBody (m_collisionAggregate, child->m_body);
}
NewtonCollisionAggregateDestroy(m_collisionAggregate);
*/
}
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);
}
void AddJoesPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin, const float animSpeed = 0.0f, const int numSegments = 4)
{
float height = 1.0f;
float width = 4.0f;
dVector size (width, height, width);
NewtonBody* parent = CreateBox (scene, origin + dVector (0.0f, 0.5f, 0.0f, 0.0f), size);
#if (defined (_USE_HARD_JOINTS) && defined (xxxxx))
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), parent, NULL);
NewtonSkeletonSetSolverMode(skeleton, 1);
#endif
for (int i=0; i<numSegments; i++)
{
float height = 1.0f;
float width = 0.5f;
dVector size (width, height, width);
NewtonBody* child = CreateBox (scene, origin + dVector (0.0f, 0.5f + height * float(i+1), 0.0f, 0.0f), size);
dMatrix matrix0 = dGetIdentityMatrix(); matrix0.m_posit = dVector (0.0f, height*-0.5f, 0.0f, 1.0f);
dMatrix matrix1 = dGetIdentityMatrix(); matrix1.m_posit = dVector (0.0f, height*0.5f, 0.0f, 1.0f);
JoesRagdollJoint* joint = new JoesRagdollJoint (child, parent, matrix0, matrix1, scene->GetNewton());
if (animSpeed != 0.0f) joint->m_anim_speed = animSpeed, joint->m_anim_offset = float(i) / float(numSegments); // animated
#if (defined (_USE_HARD_JOINTS) && defined (xxxxx))
NewtonSkeletonContainerAttachBone (skeleton, child, parent);
#endif
parent = child;
}
#if (defined (_USE_HARD_JOINTS) && defined (xxxxx))
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
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
}
