void RayCastCompoundsAllSubShapes(const dVector& origin, const dVector& end)
{
m_param = 1.0f;
m_body = NULL;
NewtonWorld* const world = GetWorld();
NewtonWorldRayCast(world, &origin[0], &end[0], PickCompound, this, NULL, 0);
if (m_body) {
// we found a compound, find all sub shape on teh path of the ray
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector localP0(matrix.UntransformVector(origin));
dVector localP1(matrix.UntransformVector(end));
NewtonCollision* const compoundCollision = NewtonBodyGetCollision(m_body);
dAssert(NewtonCollisionGetType(compoundCollision) == SERIALIZE_ID_COMPOUND);
for (void* node = NewtonCompoundCollisionGetFirstNode(compoundCollision); node; node = NewtonCompoundCollisionGetNextNode(compoundCollision, node)) {
dVector normal;
dLong attribute;
NewtonCollision* const subShape = NewtonCompoundCollisionGetCollisionFromNode(compoundCollision, node);
dFloat xxx = NewtonCollisionRayCast(subShape, &localP0[0], &localP1[0], &normal[0], &attribute);
if (xxx < 1.0f) {
dTrace (("sub shape hit\n"))
}
}
}
}
int dNewton::OnCompoundSubCollisionAABBOverlap (const NewtonMaterial* const material, const NewtonBody* const body0, const void* const collisionNode0, const NewtonBody* const body1, const void* const collisionNode1, int threadIndex)
{
dAssert (NewtonBodyGetWorld (body0) == NewtonBodyGetWorld (body1));
dNewton* const world = (dNewton*) NewtonWorldGetUserData(NewtonBodyGetWorld (body0));
dNewtonBody* const dBody0 = (dNewtonBody*) NewtonBodyGetUserData (body0);
dNewtonBody* const dBody1 = (dNewtonBody*) NewtonBodyGetUserData (body1);
NewtonCollision* const collision0 = NewtonBodyGetCollision(body0);
NewtonCollision* const collision1 = NewtonBodyGetCollision(body1);
NewtonCollision* const subCollision0 = collisionNode0 ? (NewtonCollision*) NewtonCompoundCollisionGetCollisionFromNode (collision0, (void*)collisionNode0) : collision0;
NewtonCollision* const subCollision1 = collisionNode1 ? (NewtonCollision*) NewtonCompoundCollisionGetCollisionFromNode (collision1, (void*)collisionNode1) : collision1;
dNewtonCollision* const dsubCollision0 = (dNewtonCollision*)NewtonCollisionGetUserData(subCollision0);
dNewtonCollision* const dsubCollision1 = (dNewtonCollision*)NewtonCollisionGetUserData(subCollision1);
dAssert (dsubCollision0);
dAssert (dsubCollision1);
return world->OnCompoundSubCollisionAABBOverlap (dBody0, dsubCollision0, dBody1, dsubCollision1, threadIndex);
}
void CustomPlayerController::Init(dFloat mass, dFloat outerRadius, dFloat innerRadius, dFloat height, dFloat stairStep, const dMatrix& localAxis)
{
dAssert (stairStep >= 0.0f);
dAssert (innerRadius >= 0.0f);
dAssert (outerRadius >= innerRadius);
dAssert (height >= stairStep);
dAssert (localAxis[0].m_w == dFloat (0.0f));
dAssert (localAxis[1].m_w == dFloat (0.0f));
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
SetRestrainingDistance (0.0f);
m_outerRadio = outerRadius;
m_innerRadio = innerRadius;
m_height = height;
m_stairStep = stairStep;
SetClimbSlope(45.0f * 3.1416f/ 180.0f);
m_upVector = localAxis[0];
m_frontVector = localAxis[1];
m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f);
m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f);
const int steps = 12;
dVector convexPoints[2][steps];
// create an inner thin cylinder
dFloat shapeHigh = height;
dAssert (shapeHigh > 0.0f);
dVector p0 (0.0f, m_innerRadio, 0.0f, 0.0f);
dVector p1 (shapeHigh, m_innerRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(p0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(p1));
}
NewtonCollision* const supportShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
// create the outer thick cylinder
dMatrix outerShapeMatrix (localAxis);
dFloat capsuleHigh = m_height - stairStep;
dAssert (capsuleHigh > 0.0f);
m_sphereCastOrigin = capsuleHigh * 0.5f + stairStep;
outerShapeMatrix.m_posit = outerShapeMatrix[0].Scale(m_sphereCastOrigin);
outerShapeMatrix.m_posit.m_w = 1.0f;
NewtonCollision* const bodyCapsule = NewtonCreateCapsule(world, 0.25f, 0.5f, 0, &outerShapeMatrix[0][0]);
NewtonCollisionSetScale(bodyCapsule, capsuleHigh, m_outerRadio * 4.0f, m_outerRadio * 4.0f);
// compound collision player controller
NewtonCollision* const playerShape = NewtonCreateCompoundCollision(world, 0);
NewtonCompoundCollisionBeginAddRemove(playerShape);
NewtonCompoundCollisionAddSubCollision (playerShape, supportShape);
NewtonCompoundCollisionAddSubCollision (playerShape, bodyCapsule);
NewtonCompoundCollisionEndAddRemove (playerShape);
// create the kinematic body
dMatrix locationMatrix (dGetIdentityMatrix());
m_body = NewtonCreateKinematicBody(world, playerShape, &locationMatrix[0][0]);
// players must have weight, otherwise they are infinitely strong when they collide
NewtonCollision* const shape = NewtonBodyGetCollision(m_body);
NewtonBodySetMassProperties(m_body, mass, shape);
// make the body collidable with other dynamics bodies, by default
NewtonBodySetCollidable (m_body, true);
dFloat castHigh = capsuleHigh * 0.4f;
dFloat castRadio = (m_innerRadio * 0.5f > 0.05f) ? m_innerRadio * 0.5f : 0.05f;
dVector q0 (0.0f, castRadio, 0.0f, 0.0f);
dVector q1 (castHigh, castRadio, 0.0f, 0.0f);
for (int i = 0; i < steps; i ++) {
dMatrix rotation (dPitchMatrix (i * 2.0f * 3.141592f / steps));
convexPoints[0][i] = localAxis.RotateVector(rotation.RotateVector(q0));
convexPoints[1][i] = localAxis.RotateVector(rotation.RotateVector(q1));
}
m_castingShape = NewtonCreateConvexHull(world, steps * 2, &convexPoints[0][0].m_x, sizeof (dVector), 0.0f, 0, NULL);
m_supportShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 0));
m_upperBodyShape = NewtonCompoundCollisionGetCollisionFromNode (shape, NewtonCompoundCollisionGetNodeByIndex (shape, 1));
NewtonDestroyCollision (bodyCapsule);
NewtonDestroyCollision (supportShape);
NewtonDestroyCollision (playerShape);
m_isJumping = false;
}
void CustomVehicleController::Init (NewtonCollision* const chassisShape, const dMatrix& vehicleFrame, dFloat mass, const dVector& gravityVector)
{
m_finalized = false;
m_externalContactStatesCount = 0;
m_sleepCounter = VEHICLE_SLEEP_COUNTER;
m_freeContactList = m_externalContactStatesPoll.GetFirst();
CustomVehicleControllerManager* const manager = (CustomVehicleControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
// create a compound collision
NewtonCollision* const vehShape = NewtonCreateCompoundCollision(world, 0);
NewtonCompoundCollisionBeginAddRemove(vehShape);
// if the shape is a compound collision ass all the pieces one at a time
int shapeType = NewtonCollisionGetType (chassisShape);
if (shapeType == SERIALIZE_ID_COMPOUND) {
for (void* node = NewtonCompoundCollisionGetFirstNode(chassisShape); node; node = NewtonCompoundCollisionGetNextNode (chassisShape, node)) {
NewtonCollision* const subCollision = NewtonCompoundCollisionGetCollisionFromNode(chassisShape, node);
NewtonCompoundCollisionAddSubCollision (vehShape, subCollision);
}
} else {
dAssert ((shapeType == SERIALIZE_ID_CONVEXHULL) || (shapeType == SERIALIZE_ID_BOX));
NewtonCompoundCollisionAddSubCollision (vehShape, chassisShape);
}
NewtonCompoundCollisionEndAddRemove (vehShape);
// create the rigid body for this vehicle
dMatrix locationMatrix (dGetIdentityMatrix());
m_body = NewtonCreateDynamicBody(world, vehShape, &locationMatrix[0][0]);
// set vehicle mass, inertia and center of mass
NewtonBodySetMassProperties (m_body, mass, vehShape);
// set linear and angular drag to zero
dVector drag(0.0f, 0.0f, 0.0f, 0.0f);
NewtonBodySetLinearDamping(m_body, 0);
NewtonBodySetAngularDamping(m_body, &drag[0]);
// destroy the collision help shape
NewtonDestroyCollision (vehShape);
// initialize vehicle internal components
NewtonBodyGetCentreOfMass (m_body, &m_chassisState.m_com[0]);
m_chassisState.m_comOffset = dVector (0.0f, 0.0f, 0.0f, 0.0f);
m_chassisState.m_gravity = gravityVector;
m_chassisState.m_gravityMag = dSqrt (gravityVector % gravityVector);
m_chassisState.Init(this, vehicleFrame);
// m_stateList.Append(&m_staticWorld);
m_stateList.Append(&m_chassisState);
// create the normalized size tire shape
m_tireCastShape = NewtonCreateChamferCylinder(world, 0.5f, 1.0f, 0, NULL);
// initialize all components to empty
m_engine = NULL;
m_brakes = NULL;
m_steering = NULL;
m_handBrakes = NULL;
SetDryRollingFrictionTorque (100.0f/4.0f);
SetAerodynamicsDownforceCoefficient (0.5f * dSqrt (gravityVector % gravityVector), 60.0f * 0.447f);
}
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);
}
