void LocalizationManager::createNewParticles(Particle* current,
list<Particle*>& newParticales) {
//cout << "create!" << endl;
// cout << "Create from Particle (" << current->getX()<< "," << current->getY() << "," << current->getYaw() << "," << current->getBelief() << ")" << endl;
for (int i = 0; i < 40; i++) {
double x = dRand(current->getX() - 30 < 0 ? 0 : current->getX() - 30,
current->getX() + 30 > this->_maxX ?
this->_maxX : current->getX() + 30);
double y = dRand(current->getY() - 30 < 0 ? 0 : current->getY() - 30,
current->getY() + 30 > this->_maxY ?
this->_maxY : current->getY() + 30);
double yaw = dRand(
current->getYaw() - 0.5 < 0 ? 0 : current->getYaw() - 0.5,
current->getYaw() + 0.5 > 6.2 ? 6.2 : current->getYaw() + 0.5);
//yaw = dRand(current->getYaw() - 0.5 ,current->getYaw() + 0.5);
if (yaw > M_PI) {
yaw = -(2 * M_PI - yaw);
} else if (yaw < -M_PI) {
yaw = (2 * M_PI + yaw);
}
Particle* p = new Particle(x, y, yaw, current->getBelief() - 0.0001,
current->getMap());
//p->updateBel(deltaX,deltaY,deltaYaw,laser);
//Particle* p = new Particle(x,y,yaw,-1,current->getMap());
//cout << "New Particle (" << p->getX()<< "," << p->getY() << "," << p->getYaw() << "," << p->getBelief() << ")" << endl;
//this->_particlesList.push_front(p);
newParticales.push_back(p);
}
}
void MainWindow::generatePoints()
{
for(int i=0; i<50; i++)
{
ver->push_back(dRand(0,this->height(),engine));
dir->push_back(dRand(-1,1,engine));
}
}
int dTestRand()
{
unsigned long oldseed = seed;
int ret = 1;
seed = 0;
if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
dRand() != 0x6252e503) ret = 0;
seed = oldseed;
return ret;
}
static void AddUniformScaledPrimitives (DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix)
{
// create the shape and visual mesh as a common data to be re used
NewtonWorld* const world = scene->GetNewton();
NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID);
dFloat startElevation = 1000.0f;
dMatrix matrix (dRollMatrix(-3.141592f/2.0f));
for (int i = 0; i < xCount; i ++) {
dFloat x = origin.m_x + (i - xCount / 2) * spacing;
for (int j = 0; j < zCount; j ++) {
dFloat scale = 0.75f + 1.0f * (dFloat (dRand()) / dFloat(dRAND_MAX) - 0.5f);
//scale = 1.0f;
NewtonCollisionSetScale (collision, scale, scale, scale);
DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga");
dFloat z = origin.m_z + (j - zCount / 2) * spacing;
matrix.m_posit.m_x = x;
matrix.m_posit.m_z = z;
dVector floor (FindFloor (world, dVector (matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation));
matrix.m_posit.m_y = floor.m_y + 4.f;
// create a solid
CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
// release the mesh
geometry->Release();
}
}
// do not forget to delete the collision
NewtonDestroyCollision (collision);
}
void SpawnRandomProp(const dMatrix& location) const
{
NewtonWorld* const world = GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
//scene->SetCurrent();
static PrimitiveType proSelection[] = {_SPHERE_PRIMITIVE, _BOX_PRIMITIVE, _CAPSULE_PRIMITIVE, _CYLINDER_PRIMITIVE, _CONE_PRIMITIVE,
_CHAMFER_CYLINDER_PRIMITIVE, _RANDOM_CONVEX_HULL_PRIMITIVE, _REGULAR_CONVEX_HULL_PRIMITIVE};
PrimitiveType type = PrimitiveType (dRand() % (sizeof (proSelection) / sizeof (proSelection[0])));
dVector size (0.35f, 0.25f, 0.25f, 0.0f);
NewtonCollision* const collision = CreateConvexCollision (world, dGetIdentityMatrix(), size, type, 0);
DemoMesh* const geometry = new DemoMesh("prop", collision, "smilli.tga", "smilli.tga", "smilli.tga");
dMatrix matrix (location);
matrix.m_posit.m_y += 0.5f;
NewtonBody* const prop = CreateSimpleSolid (scene, geometry, 30.0f, matrix, collision, 0);
NewtonDestroyCollision(collision);
geometry->Release();
dFloat initialSpeed = 20.0f;
dVector veloc (matrix.m_front.Scale (initialSpeed));
NewtonBodySetVelocity(prop, &veloc[0]);
NewtonBodySetLinearDamping(prop, 0);
}
// adam's all-int straightforward(?) dRandInt (0..n-1)
int dRandInt (int n)
{
// seems good; xor-fold and modulus
const unsigned long un = n;
// Since there is no memory barrier macro in ODE assign via volatile variable
// to prevent compiler reusing seed as value of `r'
volatile unsigned long raw_r = dRand();
unsigned long r = raw_r;
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
// if (un <= 0x00010000UL) {
// r ^= (r >> 16);
// if (un <= 0x00000100UL) {
// r ^= (r >> 8);
// if (un <= 0x00000010UL) {
// r ^= (r >> 4);
// if (un <= 0x00000004UL) {
// r ^= (r >> 2);
// if (un <= 0x00000002UL) {
// r ^= (r >> 1);
// }
// }
// }
// }
// }
// Optimized version of above
if (un <= 0x00000010UL) {
r ^= (r >> 16);
r ^= (r >> 8);
r ^= (r >> 4);
if (un <= 0x00000002UL) {
r ^= (r >> 2);
r ^= (r >> 1);
} else {
static void AddNonUniformScaledPrimitives(DemoEntityManager* const scene, dFloat mass, const dVector& origin, const dVector& size, int xCount, int zCount, dFloat spacing, PrimitiveType type, int materialID, const dMatrix& shapeOffsetMatrix)
{
// create the shape and visual mesh as a common data to be re used
NewtonWorld* const world = scene->GetNewton();
// NewtonCollision* const collision = CreateConvexCollision (world, &shapeOffsetMatrix[0][0], size, type, materialID);
NewtonCollision* const collision = CreateConvexCollision(world, dGetIdentityMatrix(), size, type, materialID);
dFloat startElevation = 1000.0f;
dMatrix matrix(dGetIdentityMatrix());
//matrix = dPitchMatrix(-45.0f * 3.141592f/180.0f);
for (int i = 0; i < xCount; i++) {
dFloat x = origin.m_x + (i - xCount / 2) * spacing;
for (int j = 0; j < zCount; j++) {
dFloat scalex = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);
dFloat scaley = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);
dFloat scalez = 1.0f + 1.5f * (dFloat(dRand()) / dFloat(dRAND_MAX) - 0.5f);
dFloat z = origin.m_z + (j - zCount / 2) * spacing;
matrix.m_posit.m_x = x;
matrix.m_posit.m_z = z;
dVector floor(FindFloor(world, dVector(matrix.m_posit.m_x, startElevation, matrix.m_posit.m_z, 0.0f), 2.0f * startElevation));
matrix.m_posit.m_y = floor.m_y + 8.0f;
// create a solid
//NewtonBody* const body = CreateSimpleSolid (scene, geometry, mass, matrix, collision, materialID);
NewtonBody* const body = CreateSimpleSolid(scene, NULL, mass, matrix, collision, materialID);
DemoEntity* entity = (DemoEntity*)NewtonBodyGetUserData(body);
NewtonCollisionSetScale(collision, scalex, scaley, scalez);
DemoMesh* const geometry = new DemoMesh("cylinder_1", collision, "smilli.tga", "smilli.tga", "smilli.tga");
entity->SetMesh(geometry, dGetIdentityMatrix());
NewtonBodySetCollisionScale(body, scalex, scaley, scalez);
dVector omega(0.0f);
NewtonBodySetOmega(body, &omega[0]);
// release the mesh
geometry->Release();
}
}
// do not forget to delete the collision
NewtonDestroyCollision(collision);
}
template <> OracleMongeMatrix<double>::OracleMongeMatrix(size_t r, size_t c): _rows(r), _cols(c)
{
_rowSlopes = new double[r];
_colSlopes = new double[c];
srand ( time(NULL) );
for (size_t i = 0; i < r; i++) {
_rowSlopes[i] = tan(dRand(-0.5*M_PI, 0.5*M_PI));
}
std::sort(_rowSlopes,_rowSlopes+r);
for (size_t i = 0; i < c; i++) {
_colSlopes[i] = tan(dRand(-0.5*M_PI, 0.5*M_PI));
}
std::sort(_colSlopes,_colSlopes+r);
}
static dFloat Guassian (dFloat freq)
{
dFloat r = 0.0f;
int maxCount = 2 * GAUSSIAN_BELL + 1;
for (int i = 0; i < maxCount; i ++) {
dFloat t = 2.0f * dFloat (dRand()) / dFloat(dRAND_MAX) - 1.0f;
r += t;
}
freq *= 0.5f;
return freq * r / maxCount;
}
void
ReadPixSanityTest::checkDepth(ReadPixSanityResult& r, Window& w) {
DrawingSurfaceConfig& config = *r.config;
RandomDouble dRand(35798);
int thresh = 1;
r.passDepth = true;
r.errDepth = 0.0;
for (int i = 0; i < 100 && r.passDepth; ++i) {
// Generate a random depth and use it to clear the depth buffer:
GLdouble expected = dRand.next();
glClearDepth(expected);
glClear(GL_DEPTH_BUFFER_BIT);
// Because glReadPixels won't return data of type GLdouble,
// there's no straightforward portable way to deal with
// integer depth buffers that are deeper than 32 bits or
// floating-point depth buffers that have higher precision
// than a GLfloat. Since this is just a sanity check, we'll
// use integer readback and settle for 32 bits at best.
GLuint buf[READPIX_SANITY_WIN_SIZE][READPIX_SANITY_WIN_SIZE];
glReadPixels(0, 0, READPIX_SANITY_WIN_SIZE,
READPIX_SANITY_WIN_SIZE, GL_DEPTH_COMPONENT,
GL_UNSIGNED_INT, buf);
// Now compute the error for each pixel, and record the
// worst one we find:
for (int y = 0; y < READPIX_SANITY_WIN_SIZE; ++y)
for (int x = 0; x < READPIX_SANITY_WIN_SIZE; ++x) {
GLfloat dd = abs(buf[y][x]/4294967295.0
- expected);
double err = ErrorBits(dd, config.z);
if (err > r.errDepth) {
r.xDepth = x;
r.yDepth = y;
r.errDepth = err;
r.expectedDepth = expected;
r.actualDepth = buf[y][x]/4294967295.0;
}
}
if (r.errDepth > thresh)
r.passDepth = false;
w.swap();
}
} // ReadPixSanityTest::checkDepth
// adam's all-int straightforward(?) dRandInt (0..n-1)
int dRandInt (int n)
{
// seems good; xor-fold and modulus
const unsigned long un = n;
unsigned long r = dRand();
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
if (un <= 0x00010000UL) {
r ^= (r >> 16);
if (un <= 0x00000100UL) {
r ^= (r >> 8);
if (un <= 0x00000010UL) {
r ^= (r >> 4);
if (un <= 0x00000004UL) {
r ^= (r >> 2);
if (un <= 0x00000002UL) {
r ^= (r >> 1);
}
Circle * Factory::getRandCircle() {
return new Circle(dRand(-1000, 1000), dRand(-1000, 1000), dRand(0, 500));
}
// a pseudo Gaussian random with mean 0 and variance 0.5f
dFloat dGaussianRandom (dFloat amp)
{
unsigned val;
val = dRand() + dRand();
return amp * (dFloat (val) / dFloat(dRAND_MAX) - 1.0f) * 0.5f;
}
static NewtonBody* CreateHeightFieldTerrain (DemoEntityManager* const scene, int sizeInPowerOfTwos, dFloat cellSize, dFloat elevationScale, dFloat roughness, dFloat maxElevation, dFloat minElevation)
{
int size = (1 << sizeInPowerOfTwos) + 1 ;
dFloat* const elevation = new dFloat [size * size];
//MakeFractalTerrain (elevation, sizeInPowerOfTwos, elevationScale, roughness, maxElevation, minElevation);
MakeFractalTerrain (elevation, sizeInPowerOfTwos, elevationScale, roughness, maxElevation, minElevation);
for (int i = 0; i < 4; i ++) {
ApplySmoothFilter (elevation, size);
}
//memset (elevation, 0, size * size * sizeof (dFloat));
//SetBaseHeight (elevation, size);
// apply simple calderas
// MakeCalderas (elevation, size, maxElevation * 0.7f, maxElevation * 0.1f);
// // create the visual mesh
DemoMesh* const mesh = new DemoMesh ("terrain", elevation, size, cellSize, 1.0f/4.0f, TILE_SIZE);
DemoEntity* const entity = new DemoEntity(dGetIdentityMatrix(), NULL);
scene->Append (entity);
entity->SetMesh(mesh, dGetIdentityMatrix());
mesh->Release();
// create the height field collision and rigid body
// create the attribute map
int width = size;
int height = size;
char* const attibutes = new char [size * size];
memset (attibutes, 0, width * height * sizeof (char));
NewtonCollision* collision = NewtonCreateHeightFieldCollision (scene->GetNewton(), width, height, 1, 0, elevation, attibutes, 1.0f, cellSize, 0);
#ifdef USE_STATIC_MESHES_DEBUG_COLLISION
NewtonStaticCollisionSetDebugCallback (collision, ShowMeshCollidingFaces);
#endif
NewtonCollisionInfoRecord collisionInfo;
// keep the compiler happy
memset (&collisionInfo, 0, sizeof (NewtonCollisionInfoRecord));
NewtonCollisionGetInfo (collision, &collisionInfo);
width = collisionInfo.m_heightField.m_width;
height = collisionInfo.m_heightField.m_height;
//elevations = collisionInfo.m_heightField.m_elevation;
dVector boxP0;
dVector boxP1;
// get the position of the aabb of this geometry
dMatrix matrix (entity->GetCurrentMatrix());
NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x);
matrix.m_posit = (boxP0 + boxP1).Scale (-0.5f);
matrix.m_posit.m_w = 1.0f;
//SetMatrix (matrix);
entity->ResetMatrix (*scene, matrix);
// create the terrainBody rigid body
NewtonBody* const terrainBody = NewtonCreateDynamicBody(scene->GetNewton(), collision, &matrix[0][0]);
// release the collision tree (this way the application does not have to do book keeping of Newton objects
NewtonDestroyCollision (collision);
// in newton 300 collision are instance, you need to ready it after you create a body, if you want to male call on the instance
collision = NewtonBodyGetCollision(terrainBody);
#if 0
// uncomment this to test horizontal displacement
unsigned short* const horizontalDisplacemnet = new unsigned short[size * size];
for (int i = 0; i < size * size; i++) {
horizontalDisplacemnet[i] = dRand();
}
NewtonHeightFieldSetHorizontalDisplacement(collision, horizontalDisplacemnet, 0.02f);
delete horizontalDisplacemnet;
#endif
// save the pointer to the graphic object with the body.
NewtonBodySetUserData (terrainBody, entity);
// set the global position of this body
// NewtonBodySetMatrix (m_terrainBody, &matrix[0][0]);
// set the destructor for this object
//NewtonBodySetDestructorCallback (terrainBody, Destructor);
// get the position of the aabb of this geometry
//NewtonCollisionCalculateAABB (collision, &matrix[0][0], &boxP0.m_x, &boxP1.m_x);
#ifdef USE_TEST_ALL_FACE_USER_RAYCAST_CALLBACK
// set a ray cast callback for all face ray cast
NewtonTreeCollisionSetUserRayCastCallback (collision, AllRayHitCallback);
dVector p0 (0, 100, 0, 0);
dVector p1 (0, -100, 0, 0);
dVector normal;
dLong id;
dFloat parameter;
parameter = NewtonCollisionRayCast (collision, &p0[0], &p1[0], &normal[0], &id);
#endif
delete[] attibutes;
delete[] elevation;
return terrainBody;
}
Square * Factory::getRandSquare() {
return new Square(dRand(-1000, 1000), dRand(-1000, 1000),
dRand(-1000, 1000), dRand(-1000, 1000));
}
Rect * Factory::getRandRect() {
double x1 = dRand(-1000, 1000);
double y1 = dRand(-1000, 1000);
return new Rect(x1, y1, dRand(x1, 1000), dRand(-1000, y1));
}
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);
}
Point * Factory::getRandPoint() {
return new Point(dRand(-1000, 1000), dRand(-1000, 1000));
}
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);
}
