/**
* @brief
* Constructor
*/
BodyEllipsoid::BodyEllipsoid(PLPhysics::World &cWorld, const Vector3 &vRadius) :
PLPhysics::BodyEllipsoid(cWorld, static_cast<World&>(cWorld).CreateBodyImpl(), vRadius)
{
// Deactivate the physics simulation if required
const bool bSimulationActive = cWorld.IsSimulationActive();
if (bSimulationActive)
cWorld.SetSimulationActive(false);
// Get the Newton physics world
Newton::NewtonWorld *pNewtonWorld = static_cast<World&>(cWorld).GetNewtonWorld();
// Create collision primitive
Newton::NewtonCollision *pCollision = NewtonCreateSphere(pNewtonWorld, m_vRadius.x, m_vRadius.y, m_vRadius.z, 0, nullptr);
// Create the rigid body
// [TODO] Remove this as soon as there's an up-to-date Linux version of Newton Game Dynamics available!
#if (NEWTON_MAJOR_VERSION == 2) && (NEWTON_MINOR_VERSION >= 28)
Newton::NewtonBody *pNewtonBody = NewtonCreateBody(pNewtonWorld, pCollision, Matrix4x4::Identity);
#else
Newton::NewtonBody *pNewtonBody = NewtonCreateBody(pNewtonWorld, pCollision);
#endif
NewtonReleaseCollision(pNewtonWorld, pCollision);
// Calculate the collision volume
const float fCollisionVolume = static_cast<float>((4/3)*Math::Pi*m_vRadius.x*m_vRadius.y*m_vRadius.z);
// Initialize the Newton physics body
static_cast<BodyImpl&>(GetBodyImpl()).InitializeNewtonBody(*this, *pNewtonBody, fCollisionVolume);
// Reactivate the physics simulation if required
if (bSimulationActive)
cWorld.SetSimulationActive(bSimulationActive);
}
void CollisionDetection::createObjPrimitive( Real x, Real y, Real z, Real radius)
{
dFloat pos[16];
for(int i=0; i < 16; i++) pos[i] = 0.0f;
pos[0] = 1.0f;
pos[5] = 1.0f;
pos[10] = 1.0f;
pos[15] = 1.0f;
pos[12] = x;
pos[13] = y;
pos[14] = z;
objCollision = NewtonCreateSphere (newtonWorld, radius, radius, radius, shapeID, &pos[0]);
shapeID++;
//void* ptoFunc = &(mySerializeCollisionCallbackFunction);
//NewtonReleaseCollision (newtonWorld, objCollision);
}
iPhysicsCollision* iPhysics::createSphere(float32 radius, const iaMatrixf& offset, uint64 worldID)
{
iPhysicsCollision* result = nullptr;
const NewtonWorld* world = static_cast<const NewtonWorld*>(getWorld(worldID)->getNewtonWorld());
if (world != nullptr)
{
NewtonCollision* collision = NewtonCreateSphere(static_cast<const NewtonWorld*>(world), radius, 0, offset.getData());
result = new iPhysicsCollision(collision, worldID);
NewtonCollisionSetUserID(static_cast<const NewtonCollision*>(collision), result->getID());
_collisionsListMutex.lock();
_collisions[result->getID()] = result;
_collisionsListMutex.unlock();
}
return result;
}
NewtonCollision* SphereCollider3D::CreateHandle(PhysWorld3D* world) const
{
return NewtonCreateSphere(world->GetHandle(), m_radius, 0, Matrix4f::Translate(m_position));
}
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_y, 0, NULL);
break;
}
case _CAPSULE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateCapsule (world, size.m_x * 0.5f, size.m_y, 0, NULL);
break;
}
case _TAPERED_CAPSULE_PRIMITIVE:
{
// create the collision
collision = NewtonCreateTaperedCapsule (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 _TAPERED_CYLINDER_PRIMITIVE:
{
// create the collision
collision = NewtonCreateTaperedCylinder (world, size.m_x * 0.5f, size.m_z * 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];
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 ++) {
cloud[count] = acc.RotateVector(p);
acc = acc * rotation;
count ++;
}
}
collision = NewtonCreateConvexHull (world, count, &cloud[0].m_x, sizeof (dVector), 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;
}
NzSphereGeom::NzSphereGeom(NzPhysWorld* physWorld, float radius, const NzMatrix4f& transformMatrix) :
NzBaseGeom(physWorld),
m_radius(radius)
{
m_collision = NewtonCreateSphere(physWorld->GetHandle(), radius, 0, transformMatrix);
}
cCollideShapeNewton::cCollideShapeNewton(eCollideShapeType aType, const cVector3f &avSize,
cMatrixf* apOffsetMtx, NewtonWorld* apNewtonWorld,
iPhysicsWorld *apWorld)
: iCollideShape(apWorld)
{
mpNewtonCollision = NULL;
mpNewtonWorld = apNewtonWorld;
mvSize = avSize;
mType = aType;
mfVolume = 0;
float *pMtx = NULL;
cMatrixf mtxTranspose;
if(apOffsetMtx)
{
m_mtxOffset = *apOffsetMtx;
mtxTranspose = m_mtxOffset.GetTranspose();
pMtx = &(mtxTranspose.m[0][0]);
}
else
m_mtxOffset = cMatrixf::Identity;
////////////////////////////////////////////
// Create Newton collision
switch(aType)
{
case eCollideShapeType_Null: mpNewtonCollision = NewtonCreateNull(apNewtonWorld); break;
case eCollideShapeType_Box: mpNewtonCollision = NewtonCreateBox(apNewtonWorld,
mvSize.x, mvSize.y, mvSize.z,
pMtx); break;
case eCollideShapeType_Sphere: mpNewtonCollision = NewtonCreateSphere(apNewtonWorld,
mvSize.x, mvSize.y, mvSize.z,
pMtx); break;
case eCollideShapeType_Cylinder: mpNewtonCollision = NewtonCreateCylinder(apNewtonWorld,
mvSize.x, mvSize.y,
pMtx); break;
case eCollideShapeType_Capsule: mpNewtonCollision = NewtonCreateCapsule(apNewtonWorld,
mvSize.x, mvSize.y,
pMtx); break;
}
////////////////////////////////////////////
// Calculate Bounding volume and volume.
if(mType == eCollideShapeType_Box)
{
mBoundingVolume.SetSize(mvSize);
mfVolume = mvSize.x * mvSize.y *mvSize.z;
}
else if(mType == eCollideShapeType_Sphere)
{
mBoundingVolume.SetSize(mvSize*2);
mfVolume = (4.0f / 3.0f) * kPif * (mvSize.x*mvSize.x*mvSize.x);
}
else if(mType == eCollideShapeType_Cylinder ||
mType == eCollideShapeType_Capsule)
{
mBoundingVolume.SetSize(cVector3f(mvSize.y,mvSize.x*2,mvSize.x*2));
//Not gonna be correct for capsule...
if(mType == eCollideShapeType_Cylinder)
mfVolume = kPif * (mvSize.x*mvSize.x)*mvSize.y;
else
{
//Height of the cylinder part.
float fCylHeight = mvSize.y - (mvSize.x*2);
mfVolume =0;
//The volume of the cylinder part.
if(fCylHeight>0)
mfVolume += kPif * (mvSize.x*mvSize.x)*fCylHeight;
//The volume of the sphere part.
mfVolume += (4.0f / 3.0f) * kPif * (mvSize.x*mvSize.x*mvSize.x);
}
}
mBoundingVolume.SetTransform(m_mtxOffset);
}
NewtonBody* CPhysics::CreateSphere( NewtonWorld *world, CObject3D *object, float radius, float mass /*= 0.0f*/ )
{
NewtonCollision *collision = NewtonCreateSphere(world, radius, radius, radius, 0, NULL);
return CreateRigidBody(world, object, collision, mass);
}
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);
}
dNewtonCollisionSphere::dNewtonCollisionSphere(dNewtonWorld* const world, dFloat r)
:dNewtonCollision(world, 0)
{
NewtonWaitForUpdateToFinish(m_myWorld->m_world);
SetShape(NewtonCreateSphere(m_myWorld->m_world, r, 0, NULL));
}
