NewtonCollision* CreateConvexCollision (NewtonWorld* world, const dMatrix& srcMatrix, const dVector& originalSize, PrimitiveType type, int materialID__)
{
dVector size (originalSize);
NewtonCollision* collision = NULL;
switch (type)
{
case _NULL_PRIMITIVE:
{
collision = NewtonCreateNull (world);
break;
}
case _SPHERE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateSphere (world, size.m_x * 0.5f, 0, NULL);
break;
}
case _BOX_PRIMITIVE:
{
// create the collision
collision = NewtonCreateBox (world, size.m_x, size.m_y, size.m_z, 0, NULL);
break;
}
case _CONE_PRIMITIVE:
{
dFloat r = size.m_x * 0.5f;
dFloat h = size.m_y;
// create the collision
collision = NewtonCreateCone (world, r, h, 0, NULL);
break;
}
case _CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateCylinder (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL);
break;
}
case _CAPSULE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateCapsule (world, size.m_x * 0.5f, size.m_z * 0.5f, size.m_y, 0, NULL);
break;
}
case _CHAMFER_CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateChamferCylinder (world, size.m_x * 0.5f, size.m_y, 0, NULL);
break;
}
case _RANDOM_CONVEX_HULL_PRIMITIVE:
{
// Create a clouds of random point around the origin
#define SAMPLE_COUNT 200
dVector cloud [SAMPLE_COUNT];
// make sure that at least the top and bottom are present
cloud [0] = dVector ( size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
cloud [1] = dVector (-size.m_x * 0.5f, 0.0f, 0.0f, 0.0f);
cloud [2] = dVector ( 0.0f, size.m_y * 0.5f, 0.0f, 0.0f);
cloud [3] = dVector ( 0.0f, -size.m_y * 0.5f, 0.0f, 0.0f);
cloud [4] = dVector (0.0f, 0.0f, size.m_z * 0.5f, 0.0f);
cloud [5] = dVector (0.0f, 0.0f, -size.m_z * 0.5f, 0.0f);
int count = 6;
// populate the cloud with pseudo Gaussian random points
for (int i = 6; i < SAMPLE_COUNT; i ++) {
cloud [i].m_x = RandomVariable(size.m_x);
cloud [i].m_y = RandomVariable(size.m_y);
cloud [i].m_z = RandomVariable(size.m_z);
count ++;
}
collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 0.01f, 0, NULL);
break;
}
case _REGULAR_CONVEX_HULL_PRIMITIVE:
{
// Create a clouds of random point around the origin
#define STEPS_HULL 6
//#define STEPS_HULL 3
//dVector cloud [STEPS_HULL * 4 + 256];
dFloat cloud [STEPS_HULL * 4 + 256][3];
int count = 0;
dFloat radius = size.m_y;
dFloat height = size.m_x * 0.999f;
dFloat x = - height * 0.5f;
dMatrix rotation (dPitchMatrix(2.0f * 3.141592f / STEPS_HULL));
for (int i = 0; i < 4; i ++) {
dFloat pad = ((i == 1) || (i == 2)) * 0.25f * radius;
dVector p (x, 0.0f, radius + pad);
x += 0.3333f * height;
dMatrix acc (dGetIdentityMatrix());
for (int j = 0; j < STEPS_HULL; j ++) {
dVector tmp (acc.RotateVector(p));
cloud[count][0] = tmp.m_x;
cloud[count][1] = tmp.m_y;
cloud[count][2] = tmp.m_z;
acc = acc * rotation;
count ++;
}
}
collision = NewtonCreateConvexHull (world, count, &cloud[0][0], 3 * sizeof (dFloat), 0.02f, 0, NULL);
break;
}
case _COMPOUND_CONVEX_CRUZ_PRIMITIVE:
{
//dMatrix matrix (GetIdentityMatrix());
dMatrix matrix (dPitchMatrix(15.0f * 3.1416f / 180.0f) * dYawMatrix(15.0f * 3.1416f / 180.0f) * dRollMatrix(15.0f * 3.1416f / 180.0f));
// NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]);
// NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]);
// NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]);
matrix.m_posit = dVector (size.m_x * 0.5f, 0.0f, 0.0f, 1.0f);
NewtonCollision* const collisionA = NewtonCreateBox (world, size.m_x, size.m_x * 0.25f, size.m_x * 0.25f, 0, &matrix[0][0]);
matrix.m_posit = dVector (0.0f, size.m_x * 0.5f, 0.0f, 1.0f);
NewtonCollision* const collisionB = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x, size.m_x * 0.25f, 0, &matrix[0][0]);
matrix.m_posit = dVector (0.0f, 0.0f, size.m_x * 0.5f, 1.0f);
NewtonCollision* const collisionC = NewtonCreateBox (world, size.m_x * 0.25f, size.m_x * 0.25f, size.m_x, 0, &matrix[0][0]);
collision = NewtonCreateCompoundCollision (world, 0);
NewtonCompoundCollisionBeginAddRemove(collision);
NewtonCompoundCollisionAddSubCollision (collision, collisionA);
NewtonCompoundCollisionAddSubCollision (collision, collisionB);
NewtonCompoundCollisionAddSubCollision (collision, collisionC);
NewtonCompoundCollisionEndAddRemove(collision);
NewtonDestroyCollision(collisionA);
NewtonDestroyCollision(collisionB);
NewtonDestroyCollision(collisionC);
break;
}
default: dAssert (0);
}
dMatrix matrix (srcMatrix);
matrix.m_front = matrix.m_front.Scale (1.0f / dSqrt (matrix.m_front % matrix.m_front));
matrix.m_right = matrix.m_front * matrix.m_up;
matrix.m_right = matrix.m_right.Scale (1.0f / dSqrt (matrix.m_right % matrix.m_right));
matrix.m_up = matrix.m_right * matrix.m_front;
NewtonCollisionSetMatrix(collision, &matrix[0][0]);
return collision;
}
void CustomControlledBallAndSocket::UpdateTargetMatrix ()
{
m_targetRotation = dPitchMatrix(m_targetAngles[0]) * dYawMatrix(m_targetAngles[1]) * dRollMatrix(m_targetAngles[2]);
}
void SceneCollision (DemoEntityManager* const scene)
{
NewtonWorld* world = scene->GetNewton();
// add the Sky
scene->CreateSkyBox();
// create a body and with a scene collision
NewtonCollision* const sceneCollision = NewtonCreateSceneCollision (scene->GetNewton(), 0);
// create a visual scene empty mesh
ComplexScene* const visualMesh = new ComplexScene(sceneCollision);
scene->Append (visualMesh);
// add some shapes
visualMesh->AddPrimitives(scene);
// this is optional, finish the scene construction, optimize the collision scene
dMatrix matrix (dGetIdentityMatrix());
// create the level body and add it to the world
NewtonBody* const level = NewtonCreateDynamicBody (world, sceneCollision, &matrix[0][0]);
// replace the collision with the newly created one the
visualMesh->m_sceneCollision = NewtonBodyGetCollision(level);
// set the reference to the visual
NewtonBodySetUserData(level, visualMesh);
// do not forget to release the collision
NewtonDestroyCollision (sceneCollision);
// add few objects
int defaultMaterialID = NewtonMaterialGetDefaultGroupID (world);
NewtonMaterialSetCollisionCallback (world, defaultMaterialID, defaultMaterialID, OnBodyAABBOverlap, OnContactCollision);
NewtonMaterialSetCompoundCollisionCallback(world, defaultMaterialID, defaultMaterialID, OnSubShapeAABBOverlapTest);
dVector location (0.0f, 0.0f, 0.0f, 0.0f);
dVector size (0.5f, 0.5f, 0.5f, 0.0f);
dMatrix shapeOffsetMatrix (dGetIdentityMatrix());
int count = 5;
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
AddPrimitiveArray(scene, 10.0f, location, size, count, count, 1.7f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix);
dMatrix camMatrix (dRollMatrix(-20.0f * 3.1416f /180.0f) * dYawMatrix(-45.0f * 3.1416f /180.0f));
dQuaternion rot (camMatrix);
dVector origin (-15.0f, 5.0f, 0.0f, 0.0f);
scene->SetCameraMatrix(rot, origin);
}
static void DestroyThisBodyCallback (const NewtonBody* body, const NewtonJoint* contactJoint)
{
NewtonWorld* world;
NewtonMesh* topMesh;
NewtonMesh* bottomMesh;
NewtonMesh* effectMesh;
RenderPrimitive* srcPrimitive;
dMatrix matrix;
dFloat maxForce;
dVector point;
dVector dir0;
dVector dir1;
// Get the world;
world = NewtonBodyGetWorld (body);
// find a the strongest force
maxForce = 0.0f;
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dVector force;
NewtonMaterial* material;
material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce (material, &force.m_x);
if (force.m_x > maxForce) {
dVector normal;
NewtonMaterialGetContactPositionAndNormal(material, &point[0], &normal[0]);
NewtonMaterialGetContactTangentDirections (material, &dir0[0], &dir0[0]);
}
}
// get the visual primitive
srcPrimitive = (RenderPrimitive*) NewtonBodyGetUserData (body);
// get the effect mesh that is use to create the debris pieces
effectMesh = srcPrimitive->m_specialEffect;
// calculate the cut plane plane
NewtonBodyGetMatrix (body, &matrix[0][0]);
dMatrix clipMatrix (dgGrammSchmidt(dir0) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
clipMatrix.m_posit = point;
clipMatrix.m_posit.m_w = 1.0f;
clipMatrix = clipMatrix * matrix.Inverse();
// break the mesh into two pieces
NewtonMeshClip (effectMesh, meshClipper, &clipMatrix[0][0], &topMesh, &bottomMesh);
if (topMesh && bottomMesh) {
dFloat volume;
NewtonMesh* meshPartA = NULL;
NewtonMesh* meshPartB = NULL;
volume = NewtonConvexCollisionCalculateVolume (NewtonBodyGetCollision(body));
// the clipper was able to make a cut now we can create new debris piece for replacement
dMatrix clipMatrix1 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (bottomMesh, meshClipper, &clipMatrix1[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, bottomMesh, volume);
}
NewtonMeshDestroy(bottomMesh);
dMatrix clipMatrix2 (dgGrammSchmidt(dir1) *
dYawMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)) *
dRollMatrix(30.0f * 3.1416f/180.0f * RandomVariable(1.0f)));
NewtonMeshClip (topMesh, meshClipper, &clipMatrix2[0][0], &meshPartA, &meshPartB);
if (meshPartA && meshPartB) {
// creat another split (you can make as many depend of the FPS)
CreateDebriPiece (body, meshPartA, volume);
CreateDebriPiece (body, meshPartB, volume);
NewtonMeshDestroy(meshPartA);
NewtonMeshDestroy(meshPartB);
} else {
CreateDebriPiece (body, topMesh, volume);
}
NewtonMeshDestroy(topMesh);
// remove the old visual from graphics world
SceneManager* system = (SceneManager*) NewtonWorldGetUserData(world);
delete srcPrimitive;
system->Remove(srcPrimitive);
// finally destroy this body;
NewtonDestroyBody(world, body);
}
}
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
}
void MakeHexapod(DemoEntityManager* const scene, const dMatrix& location)
{
dFloat mass = 30.0f;
// make the kinematic solver
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
// make the root body
dMatrix baseMatrix(dGetIdentityMatrix());
baseMatrix.m_posit.m_y += 0.35f;
dVector size (1.3f, 0.31f, 0.5f, 0.0f);
NewtonBody* const hexaBody = CreateBox(scene, baseMatrix * location, size, mass, 1.0f);
void* const hexaBodyNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, hexaBody);
int legEffectorCount = 0;
dCustomInverseDynamicsEffector* legEffectors[32];
baseMatrix.m_posit.m_y -= 0.06f;
// make the hexapod six limbs
for (int i = 0; i < 3; i ++) {
dMatrix rightLocation (baseMatrix);
rightLocation.m_posit += rightLocation.m_right.Scale (size.m_z * 0.65f);
rightLocation.m_posit += rightLocation.m_front.Scale (size.m_x * 0.3f - size.m_x * i / 3.0f);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, rightLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
dMatrix similarTransform (dGetIdentityMatrix());
similarTransform.m_posit.m_x = rightLocation.m_posit.m_x;
similarTransform.m_posit.m_y = rightLocation.m_posit.m_y;
dMatrix leftLocation (rightLocation * similarTransform.Inverse() * dYawMatrix(dPi) * similarTransform);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, leftLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
}
// finalize inverse dynamics solver
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
// create a fix pose frame generator
dEffectorTreeFixPose* const idlePose = new dEffectorTreeFixPose(hexaBody);
dEffectorTreeFixPose* const walkPoseGenerator = new dEffectorWalkPoseGenerator(hexaBody);
m_walkIdleBlender = new dEffectorBlendIdleWalk (hexaBody, idlePose, walkPoseGenerator);
m_postureModifier = new dAnimationHipController(m_walkIdleBlender);
m_animTreeNode = new dEffectorTreeRoot(hexaBody, m_postureModifier);
dMatrix rootMatrix;
NewtonBodyGetMatrix (hexaBody, &rootMatrix[0][0]);
rootMatrix = rootMatrix.Inverse();
for (int i = 0; i < legEffectorCount; i++) {
dEffectorTreeInterface::dEffectorTransform frame;
dCustomInverseDynamicsEffector* const effector = legEffectors[i];
dMatrix effectorMatrix(effector->GetBodyMatrix());
dMatrix poseMatrix(effectorMatrix * rootMatrix);
frame.m_effector = effector;
frame.m_posit = poseMatrix.m_posit;
frame.m_rotation = dQuaternion(poseMatrix);
idlePose->GetPose().Append(frame);
walkPoseGenerator->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
}
}
void dPluginCamera::DrawGizmo(dSceneRender* const render, int font) const
{
render->DisableZbuffer();
//render->EnableZbuffer();
render->EnableBackFace();
render->DisableLighting ();
render->DisableTexture();
render->DisableBlend();
// calculate gizmo size
dFloat zbuffer = 0.5f;
// calculate a point the lower left corner of the screen at the front plane in global space
dFloat x0 = 40.0f;
dFloat y0 = render->GetViewPortHeight() - 30.0f;
dVector origin (render->ScreenToGlobal(dVector (x0, y0, zbuffer, 1.0f)));
dFloat length = 30.0f;
dVector p1 (render->ScreenToGlobal(dVector (x0 + length, y0, zbuffer, 1.0f)));
dVector p1p0(p1 - origin);
length = dSqrt (p1p0.DotProduct3(p1p0));
// display x axis
{
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (1.0f, 0.0f, 0.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "x");
render->PopMatrix();
}
// display y axis
{
dMatrix matrix (dRollMatrix((90.0f * 3.141592f / 180.0f)));
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (0.0f, 1.0f, 0.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "y");
render->PopMatrix();
}
// display z axis
{
dMatrix matrix (dYawMatrix((-90.0f * 3.141592f / 180.0f)));
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (0.0f, 0.0f, 1.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "z");
render->PopMatrix();
}
}
StupidComplexOfConvexShapes (DemoEntityManager* const scene, int count)
:DemoEntity (dGetIdentityMatrix(), NULL)
,m_rayP0(0.0f, 0.0f, 0.0f, 0.0f)
,m_rayP1(0.0f, 0.0f, 0.0f, 0.0f)
{
scene->Append(this);
count = 40;
//count = 1;
const dFloat size = 0.5f;
DemoMesh* gemetries[32];
NewtonCollision* collisionArray[32];
NewtonWorld* const world = scene->GetNewton();
int materialID = NewtonMaterialGetDefaultGroupID(world);
// create a pool of predefined convex mesh
// PrimitiveType selection[] = {_SPHERE_PRIMITIVE, _BOX_PRIMITIVE, _CAPSULE_PRIMITIVE, _CYLINDER_PRIMITIVE, _CONE_PRIMITIVE, _TAPERED_CAPSULE_PRIMITIVE, _TAPERED_CYLINDER_PRIMITIVE, _CHAMFER_CYLINDER_PRIMITIVE, _RANDOM_CONVEX_HULL_PRIMITIVE, _REGULAR_CONVEX_HULL_PRIMITIVE};
PrimitiveType selection[] = {_SPHERE_PRIMITIVE};
for (int i = 0; i < int (sizeof (collisionArray) / sizeof (collisionArray[0])); i ++) {
int index = dRand() % (sizeof (selection) / sizeof (selection[0]));
dVector shapeSize (size + RandomVariable (size / 2.0f), size + RandomVariable (size / 2.0f), size + RandomVariable (size / 2.0f), 0.0f);
shapeSize = dVector(size, size, size, 0.0f);
collisionArray[i] = CreateConvexCollision (world, dGetIdentityMatrix(), shapeSize, selection[index], materialID);
gemetries[i] = new DemoMesh("geometry", collisionArray[i], "wood_4.tga", "wood_4.tga", "wood_1.tga");
}
// make a large complex of plane by adding lost of these shapes at a random location and oriention;
NewtonCollision* const compound = NewtonCreateCompoundCollision (world, materialID);
NewtonCompoundCollisionBeginAddRemove(compound);
for (int i = 0 ; i < count; i ++) {
for (int j = 0 ; j < count; j ++) {
float pitch = RandomVariable (1.0f) * 2.0f * 3.1416f;
float yaw = RandomVariable (1.0f) * 2.0f * 3.1416f;
float roll = RandomVariable (1.0f) * 2.0f * 3.1416f;
float x = size * (j - count / 2) + RandomVariable (size * 0.5f);
float y = RandomVariable (size * 2.0f);
float z = size * (i - count / 2) + RandomVariable (size * 0.5f);
dMatrix matrix (dPitchMatrix (pitch) * dYawMatrix (yaw) * dRollMatrix (roll));
matrix.m_posit = dVector (x, y, z, 1.0f);
int index = dRand() % (sizeof (selection) / sizeof (selection[0]));
DemoEntity* const entity = new DemoEntity(matrix, this);
entity->SetMesh(gemetries[index], dGetIdentityMatrix());
NewtonCollisionSetMatrix (collisionArray[index], &matrix[0][0]);
NewtonCompoundCollisionAddSubCollision (compound, collisionArray[index]);
}
}
NewtonCompoundCollisionEndAddRemove(compound);
CreateSimpleBody (world, NULL, 0.0f, dGetIdentityMatrix(), compound, 0);
// destroy all collision shapes after they are used
NewtonDestroyCollision(compound);
for (int i = 0; i < int (sizeof (collisionArray) / sizeof (collisionArray[0])); i ++) {
gemetries[i]->Release();
NewtonDestroyCollision(collisionArray[i]);
}
// now make and array of collision shapes for convex casting by mouse point click an drag
CreateCastingShapes(scene, size * 2.0f);
}
// This function read KeyBoard and Mouse control to control this scene
// The control are read at the simulation rate, and two states are kept
void InputControl (NewtonWorld* world)
{
// read the mouse position and set the camera direction
static dVector mouse0 (GetMousePos());
dVector mouse1 (GetMousePos());
gPrevYawAngle = gYawAngle;
gPrevRollAngle = gRollAngle;
// we are not in mouse pick mode, then we are in camera tracking mode
if (IsKeyDown (KeyCode_L_BUTTON)) {
// when click left mouse button the first time, we reset the camera
// convert the mouse x position to delta yaw angle
if (mouse1.m_x > (mouse0.m_x + 1)) {
gYawAngle += 1.0f * 3.1416f / 180.0f;
if (gYawAngle > (360.0f * 3.1416f / 180.0f)) {
gYawAngle -= (360.0f * 3.1416f / 180.0f);
}
} else if (mouse1.m_x < (mouse0.m_x - 1)) {
gYawAngle -= 1.0f * 3.1416f / 180.0f;
if (gYawAngle < 0.0f) {
gYawAngle += (360.0f * 3.1416f / 180.0f);
}
}
if (mouse1.m_y > (mouse0.m_y + 1)) {
gRollAngle += 1.0f * 3.1416f / 180.0f;
if (gRollAngle > (80.0f * 3.1416f / 180.0f)) {
gRollAngle = 80.0f * 3.1416f / 180.0f;
}
} else if (mouse1.m_y < (mouse0.m_y - 1)) {
gRollAngle -= 1.0f * 3.1416f / 180.0f;
if (gRollAngle < -(80.0f * 3.1416f / 180.0f)) {
gRollAngle = -80.0f * 3.1416f / 180.0f;
}
}
dMatrix cameraDirMat (dRollMatrix(gRollAngle) * dYawMatrix(gYawAngle));
gCurrCameraDir = cameraDirMat.m_front;
}
// save mouse position and left mouse key state for next frame
mouse0 = mouse1;
// camera control
gPrevCameraEyepoint = gCameraEyepoint;
if (IsKeyDown ('W')) {
gCameraEyepoint += gCurrCameraDir.Scale (CAMERA_SPEED / 60.0f);
} else if (IsKeyDown ('S')) {
gCameraEyepoint -= gCurrCameraDir.Scale (CAMERA_SPEED / 60.0f);
}
if (IsKeyDown ('D')) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (gCurrCameraDir * up);
gCameraEyepoint += right.Scale (CAMERA_SPEED / 60.0f);
} else if (IsKeyDown ('A')) {
dVector up (0.0f, 1.0f, 0.0f);
dVector right (gCurrCameraDir * up);
gCameraEyepoint -= right.Scale (CAMERA_SPEED / 60.0f);
}
}
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep)
{
// update the camera;
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
dMatrix targetMatrix (m_camera->GetNextMatrix());
int mouseX;
int mouseY;
scene->GetMousePosition (mouseX, mouseY);
// slow down the Camera if we have a Body
dFloat slowDownFactor = scene->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f;
// do camera translation
if (scene->GetKeyState ('W')) {
targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('S')) {
targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('A')) {
targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('D')) {
targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('Q')) {
targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
if (scene->GetKeyState ('E')) {
targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor);
}
// do camera rotation, only if we do not have anything picked
bool buttonState = m_mouseLockState || scene->GetMouseKeyState(0);
if (!m_targetPicked && buttonState) {
int mouseSpeedX = mouseX - m_mousePosX;
int mouseSpeedY = mouseY - m_mousePosY;
if (mouseSpeedX > 0) {
m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f);
} else if (mouseSpeedX < 0){
m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f);
}
if (mouseSpeedY > 0) {
m_pitch += m_pitchRate;
} else if (mouseSpeedY < 0){
m_pitch -= m_pitchRate;
}
m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f));
}
m_mousePosX = mouseX;
m_mousePosY = mouseY;
dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw));
dQuaternion rot (matrix);
m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit);
UpdatePickBody(scene, timestep);
}
void Custom6DOF::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// add the linear limits
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
dVector dp (p0 - p1);
for (int i = 0; i < 3; i ++) {
if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) {
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
// it is a limited linear dof, check if it pass the limits
dFloat dist = dp % matrix1[i];
if (dist > m_maxLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1) % matrix1[i];
if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
} else if (dist < m_minLinearLimits[i]) {
dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i]));
// clamp the error, so the not too much energy is added when constraint violation occurs
dFloat maxDist = (p0 - q1) % matrix1[i];
if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) {
q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION);
}
NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
// allow the object to return but not to kick going forward
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
}
dVector euler0;
dVector euler1;
dMatrix localMatrix (matrix0 * matrix1.Inverse());
localMatrix.GetEulerAngles(euler0, euler1);
AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch);
AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch);
if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) {
euler0 = euler1;
}
dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f);
//dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f));
bool limitViolation = false;
for (int i = 0; i < 3; i ++) {
if (euler[i] < m_minAngularLimits[i]) {
limitViolation = true;
euler[i] = m_minAngularLimits[i];
} else if (euler[i] > m_maxAngularLimits[i]) {
limitViolation = true;
euler[i] = m_maxAngularLimits[i];
}
}
if (limitViolation) {
//dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle));
dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2]));
dMatrix baseMatrix (p0y0r0 * matrix1);
dMatrix rotation (matrix0.Inverse() * baseMatrix);
dQuaternion quat (rotation);
if (quat.m_q0 > dFloat (0.99995f)) {
//dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]);
//NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]);
} else {
dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f)));
dVector p0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH));
dVector p1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH)));
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &basis[2][0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
//dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH));
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]);
//NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]);
}
}
}
static void MakeFunnyCompound (DemoEntityManager* const scene, const dVector& origin)
{
NewtonWorld* const world = scene->GetNewton();
// create an empty compound collision
NewtonCollision* const compound = NewtonCreateCompoundCollision (world, 0);
#if 1
NewtonCompoundCollisionBeginAddRemove(compound);
// add a bunch of convex collision at random position and orientation over the surface of a big sphere
float radio = 5.0f;
for (int i = 0 ; i < 300; i ++) {
NewtonCollision* collision = NULL;
float pitch = RandomVariable (1.0f) * 2.0f * 3.1416f;
float yaw = RandomVariable (1.0f) * 2.0f * 3.1416f;
float roll = RandomVariable (1.0f) * 2.0f * 3.1416f;
float x = RandomVariable (0.5f);
float y = RandomVariable (0.5f);
float z = RandomVariable (0.5f);
if ((x == 0.0f) && (y == 0.0f) && (z == 0.0f)){
x = 0.1f;
}
dVector p (x, y, z, 1.0f) ;
p = p.Scale (radio / dSqrt (p % p));
dMatrix matrix (dPitchMatrix (pitch) * dYawMatrix (yaw) * dRollMatrix (roll));
matrix.m_posit = p;
int r = dRand();
switch ((r >>2) & 3)
{
case 0:
{
collision = NewtonCreateSphere(world, 0.5, 0, &matrix[0][0]) ;
break;
}
case 1:
{
collision = NewtonCreateCapsule(world, 0.3f, 0.2f, 0.5f, 0, &matrix[0][0]) ;
break;
}
case 2:
{
collision = NewtonCreateCylinder(world, 0.25, 0.5, 0.25, 0, &matrix[0][0]) ;
break;
}
case 3:
{
collision = NewtonCreateCone(world, 0.25, 0.25, 0, &matrix[0][0]) ;
break;
}
}
dAssert (collision);
// we can set a collision id, and use data per sub collision
NewtonCollisionSetUserID(collision, i);
NewtonCollisionSetUserData(collision, (void*) i);
// add this new collision
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
}
// finish adding shapes
NewtonCompoundCollisionEndAddRemove(compound);
{
// remove the first 10 shapes
// test remove shape form a compound
NewtonCompoundCollisionBeginAddRemove(compound);
void* node = NewtonCompoundCollisionGetFirstNode(compound);
for (int i = 0; i < 10; i ++) {
//NewtonCollision* const collision = NewtonCompoundCollisionGetCollisionFromNode(compound, node);
void* const nextNode = NewtonCompoundCollisionGetNextNode(compound, node);
NewtonCompoundCollisionRemoveSubCollision(compound, node);
node = nextNode;
}
// finish remove
void* handle1 = NewtonCompoundCollisionGetNodeByIndex (compound, 30);
void* handle2 = NewtonCompoundCollisionGetNodeByIndex (compound, 100);
NewtonCollision* const shape1 = NewtonCompoundCollisionGetCollisionFromNode (compound, handle1);
NewtonCollision* const shape2 = NewtonCompoundCollisionGetCollisionFromNode (compound, handle2);
NewtonCollision* const copyShape1 = NewtonCollisionCreateInstance (shape1);
NewtonCollision* const copyShape2 = NewtonCollisionCreateInstance (shape2);
// you can also remove shape by their index
NewtonCompoundCollisionRemoveSubCollisionByIndex (compound, 30);
NewtonCompoundCollisionRemoveSubCollisionByIndex (compound, 100);
handle1 = NewtonCompoundCollisionAddSubCollision (compound, copyShape1);
handle2 = NewtonCompoundCollisionAddSubCollision (compound, copyShape2);
NewtonDestroyCollision(copyShape1);
NewtonDestroyCollision(copyShape2);
NewtonCompoundCollisionEndAddRemove(compound);
}
{
// show how to modify the children of a compound collision
NewtonCompoundCollisionBeginAddRemove(compound);
for (void* node = NewtonCompoundCollisionGetFirstNode(compound); node; node = NewtonCompoundCollisionGetNextNode(compound, node)) {
NewtonCollision* const collision = NewtonCompoundCollisionGetCollisionFromNode(compound, node);
// you can scale, change the matrix, change the inertia, do anything you want with the change
NewtonCollisionSetUserData(collision, NULL);
}
NewtonCompoundCollisionEndAddRemove(compound);
}
// NewtonCollisionSetScale(compound, 0.5f, 0.25f, 0.125f);
#else
//test Yeside compound shape shape
// - Rotation="1.5708 -0 0" Translation="0 0 0.024399" Size="0.021 0.096" Pos="0 0 0.115947"
// - Rotation="1.5708 -0 0" Translation="0 0 0.056366" Size="0.195 0.024" Pos="0 0 0.147914"
// - Rotation="1.5708 -0 0" Translation="0 0 -0.056366" Size="0.0065 0.07 Pos="0 0 0.035182"
NewtonCompoundCollisionBeginAddRemove(compound);
NewtonCollision* collision;
dMatrix offsetMatrix (dPitchMatrix(1.5708f));
offsetMatrix.m_posit.m_z = 0.115947f;
collision = NewtonCreateCylinder (world, 0.021f, 0.096f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
offsetMatrix.m_posit.m_z = 0.035182f;
collision = NewtonCreateCylinder (world, 0.0065f, 0.07f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
offsetMatrix.m_posit.m_z = 0.147914f;
collision = NewtonCreateCylinder (world, 0.195f, 0.024f, 0, &offsetMatrix[0][0]) ;
NewtonCompoundCollisionAddSubCollision (compound, collision);
NewtonDestroyCollision(collision);
NewtonCompoundCollisionEndAddRemove(compound);
#endif
// for now we will simple make simple Box, make a visual Mesh
DemoMesh* const visualMesh = new DemoMesh ("big ball", compound, "metal_30.tga", "metal_30.tga", "metal_30.tga");
int instaceCount = 2;
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
for (int ix = 0; ix < instaceCount; ix ++) {
for (int iz = 0; iz < instaceCount; iz ++) {
dFloat y = origin.m_y;
dFloat x = origin.m_x + (ix - instaceCount/2) * 15.0f;
dFloat z = origin.m_z + (iz - instaceCount/2) * 15.0f;
matrix.m_posit = FindFloor (world, dVector (x, y + 10.0f, z, 0.0f), 20.0f); ;
matrix.m_posit.m_y += 15.0f;
CreateSimpleSolid (scene, visualMesh, 10.0f, matrix, compound, 0);
}
}
visualMesh->Release();
NewtonDestroyCollision(compound);
}
void CreateBasicGait (const dVector& posit, dFloat angle)
{
dCustomActiveCharacterController* const controller = CreateTransformController();
NewtonBody* const root = ParseRagdollFile("balancingGait.txt", controller);
dMatrix bodyMatrix;
NewtonBodyGetMatrix (root, &bodyMatrix[0][0]);
bodyMatrix = dYawMatrix(angle) * bodyMatrix;
bodyMatrix.m_posit = posit;
bodyMatrix.m_posit.m_w = 1.0f;
NewtonBodySetMatrixRecursive (root, &bodyMatrix[0][0]);
/*
return;
dCustomHinge* xxx = new dCustomHinge(bodyMatrix, root);
xxx->SetFriction (20000.0f);
xxx->SetLimits(0.0f, 0.0f);
void* const rootNode = controller->AddRoot(root);
int stack = 1;
void* stackPool[128];
stackPool[0] = rootNode;
dString pevisEffector ("Bip01_Pelvis");
dString leftFootEffector ("Bip01_L_Foot");
dString rightFootEffector ("Bip01_R_Foot");
dTree<int, NewtonJoint*> filter;
while (stack) {
stack --;
void* const node = stackPool[stack];
NewtonBody* const body = controller->GetBody(node);
DemoEntity* const entity = (DemoEntity*) NewtonBodyGetUserData(body);
// add the end effectors.
if (entity->GetName() == pevisEffector) {
// root effect has it pivot at the center of mass
dVector com;
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
NewtonBodyGetCentreOfMass(body, &com[0]);
matrix.m_posit = matrix.TransformVector(com);
controller->AddEndEffector(node, matrix);
} else if (entity->GetName() == leftFootEffector) {
// all other effector are centered and oriented a the joint pivot
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
dCustomJoint* const joint = controller->GetJoint(node);
dAssert (joint->GetBody0() == body);
matrix = joint->GetMatrix0() * matrix;
dCustomRagdollMotor_EndEffector* const effector = controller->AddEndEffector(node, matrix);
dCustomKinematicController* xxx = new dCustomKinematicController (body, matrix);
matrix.m_posit.m_z -= 0.4f;
matrix.m_posit.m_x -= 0.2f;
matrix.m_posit.m_y += 0.0f;
effector->SetTargetMatrix(matrix);
xxx->SetPickMode(0);
xxx->SetTargetMatrix(matrix);
xxx->SetMaxLinearFriction (5000.0f);
xxx->SetMaxAngularFriction (5000.0f);
} else if (entity->GetName() == rightFootEffector) {
// all other effector are centered and oriented a the joint pivot
dMatrix matrix;
NewtonBodyGetMatrix(body, &matrix[0][0]);
dCustomJoint* const joint = controller->GetJoint(node);
dAssert(joint->GetBody0() == body);
matrix = joint->GetMatrix0() * matrix;
dCustomRagdollMotor_EndEffector* const effector = controller->AddEndEffector(node, matrix);
dCustomKinematicController* xxx = new dCustomKinematicController (body, matrix);
matrix.m_posit.m_z += 0.4f;
matrix.m_posit.m_x += 0.2f;
matrix.m_posit.m_y += 0.0f;
effector->SetTargetMatrix(matrix);
xxx->SetPickMode(0);
xxx->SetTargetMatrix(matrix);
xxx->SetMaxLinearFriction(5000.0f);
xxx->SetMaxAngularFriction(5000.0f);
}
for (NewtonJoint* newtonJoint = NewtonBodyGetFirstJoint(body); newtonJoint; newtonJoint = NewtonBodyGetNextJoint(body, newtonJoint)) {
if (!filter.Find(newtonJoint)) {
filter.Insert(newtonJoint);
dCustomJoint* const customJoint = (dCustomJoint*)NewtonJointGetUserData(newtonJoint);
if (customJoint->IsType(dCustomRagdollMotor::GetType())) {
dCustomRagdollMotor* const ragDollMotor = (dCustomRagdollMotor*)customJoint;
void* const bone = controller->AddBone(ragDollMotor, node);
//ragDollMotor->SetMode(true);
stackPool[stack] = bone;
stack ++;
}
}
}
}
controller->Finalize();
*/
}
