Stacking of convex hull shapes

Post Reply
Kruno
Posts: 2
Joined: Fri Mar 17, 2017 6:53 pm

Stacking of convex hull shapes

Post by Kruno »

Hello all,
I am new to bullet and fairly to C++ also.
I am trying to simulate stacking of convex hull shapes.
I want to make a container and drop shape one at the time while the rest of them (that are at the bottom already) can be fixed.
Another idea is to generate sparsely placed shapes inside the container and turn on gravity so they fall and find their place.
I don't know how to make graphic debugger so I was following the position of particles to debug.

First I tried with the second idea and I made a loop that would insert one particle at the top and run a simulation of dropping it. It worked but it seamed like previous ones are vanishing. All of them end up at the bottom.
Then I tried making a compound of bunch of them and running the simulation but i get "Segmentation fault: 11" error short after simulation starts to run.
Unfortunately I don't know how to monitor each body's position in compound.

I generate convex hull shape with another library and export vertices.

Here is where I got so far:

I generate a world

Code: Select all

// BULLET WORLD ************************************************************
    
    // WORLD CREATION ****************************************************************
    // X vector looking right, Y vector looking up, Z vector looking forward
    // (relative to the screen)
    
    // Broadphase (bounded boxes for faster collision detection)
    btBroadphaseInterface* broadphase = new btDbvtBroadphase();
    // Collision dispatcher registers a callback that filters overlapping
    // broadphase proxies so that the collisions are not processed by the rest
    // of the system
    btDefaultCollisionConfiguration* collisionConfiguration = 
        new btDefaultCollisionConfiguration();
    btCollisionDispatcher* dispatcher =
        new btCollisionDispatcher(collisionConfiguration);
    // Collision algorithm
    btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);
    // Solver
    btSequentialImpulseConstraintSolver* solver =
        new btSequentialImpulseConstraintSolver;
    // Create a dynamic world
    btDiscreteDynamicsWorld dynamicsWorld
    (
        dispatcher,
        broadphase,
        solver,
        collisionConfiguration
    );
    // Set gravity
    dynamicsWorld.setGravity(btVector3(0.f, g, 0.f));
I generate a box made of walls. Box has one corner at [0,0,0]:

Code: Select all

    // GROUND (y = 0)
    // Collision shapes are for collisions only and have no concept about the
    // mass or inertia
    // Create collision shape for ground with normal vector (0,1,0) at the
    // origin
    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0.f, 1.f, 0.f), 0.f);
    // Add ground collision shape with angular orientation (quaternion [x, y, z, w])
    // and the position vector
    btDefaultMotionState* groundMotionState = new btDefaultMotionState
    (
     btTransform(btQuaternion(0.f, 0.f, 0.f, 1.f), btVector3(0.f, 0.f, 0.f))
     );
    // Define mass and inertia of a ground (first and last parameter of the
    // constructor). Mass of 0 makes body with infinite mass and non-moveable
    btRigidBody::btRigidBodyConstructionInfo
    groundRigidBodyCI
    (
     0.f, // Mass
     groundMotionState,
     groundShape,
     btVector3(0.f, 4.f, 0.f) // Position vector
     );
    groundRigidBodyCI.m_friction = friction;
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    dynamicsWorld.addRigidBody(groundRigidBody);
    
    // WALL Left (n = [1, 0, 0])(x = 0)
    btCollisionShape* wallLShape = new btStaticPlaneShape(btVector3(1.f, 0.f, 0.f), 0.f);
    btDefaultMotionState* wallLMotionState = new btDefaultMotionState
    (
     btTransform(btQuaternion(0.f, 0.f, 0.f, 1.f), btVector3(0.f, 0.f, 0.f))
     );
    btRigidBody::btRigidBodyConstructionInfo
    wallLRigidBodyCI
    (
     0.f, // Mass
     wallLMotionState,
     wallLShape,
     btVector3(0.f, 0.f, 0.f) // Position vector
     );
    btRigidBody* wallLRigidBody = new btRigidBody(wallLRigidBodyCI);
    dynamicsWorld.addRigidBody(wallLRigidBody);
    
    // WALL Right (n = [-1, 0, 0])(x = bulletBoxX)
    btCollisionShape* wallRShape = new btStaticPlaneShape(btVector3(1.f, 0.f, 0.f), 0.f);
    btDefaultMotionState* wallRMotionState = new btDefaultMotionState
    (
     btTransform(btQuaternion(0.f, 0.f, 0.f, 1.f), btVector3(0.f, 0.f, 0.f))
     );
    btRigidBody::btRigidBodyConstructionInfo
    wallRRigidBodyCI
    (
     0.f, // Mass
     wallRMotionState,
     wallRShape,
     btVector3(bulletBoxX, 0.f, 0.f) // Position vector
     );
    btRigidBody* wallRRigidBody = new btRigidBody(wallRRigidBodyCI);
    dynamicsWorld.addRigidBody(wallRRigidBody);
    
    // WALL Front (n = [0, 0, 1])(z = 0)
    btCollisionShape* wallFShape = new btStaticPlaneShape(btVector3(0.f, 0.f, 1.f), 0.f);
    btDefaultMotionState* wallFMotionState = new btDefaultMotionState
    (
     btTransform(btQuaternion(0.f, 0.f, 0.f, 1.f), btVector3(0.f, 0.f, 0.f))
     );
    btRigidBody::btRigidBodyConstructionInfo
    wallFRigidBodyCI
    (
     0.f, // Mass
     wallFMotionState,
     wallFShape,
     btVector3(0.f, 0.f, 0.f) // Position vector
     );
    btRigidBody* wallFRigidBody = new btRigidBody(wallFRigidBodyCI);
    dynamicsWorld.addRigidBody(wallFRigidBody);
    
    // WALL Back (n = [0, 0, -1])(z = bulletBoxZ)
    btCollisionShape* wallBShape = new btStaticPlaneShape(btVector3(0.f, 0.f, 1.f), 0.f);
    btDefaultMotionState* wallBMotionState = new btDefaultMotionState
    (
     btTransform(btQuaternion(0.f, 0.f, 0.f, 1.f), btVector3(0.f, 0.f, 0.f))
     );
    btRigidBody::btRigidBodyConstructionInfo
    wallBRigidBodyCI
    (
     0.f, // Mass
     wallBMotionState,
     wallBShape,
     btVector3(0.f, 0.f, bulletBoxZ) // Position vector
     );
    btRigidBody* wallBRigidBody = new btRigidBody(wallBRigidBodyCI);
    dynamicsWorld.addRigidBody(wallBRigidBody);
Then I start the loop that would introduce one particle at the time and simulate it's fall. All I need is particles start and finis location:

Code: Select all

// PARTICLES STACKING ********************************************************
    // Create objects for position variables
    btVector3 hullC0;      // Position at the start of the simulation
    btVector3 hullC;        // Position at the end of the simulation
    btVector3 hullRot0;    // Rotation at the beginning of the simulation
    btVector3 hullRot;     // Rotation at the end of the simulation
    // Define quaternion
    btQuaternion hullQuant;
    btQuaternion duplicateHullQuant;
    
    voro::voronoicell v;     // Object to store convex hull points in with another library

    // Vectors to store rigid bodies and all of its objects in
    std::vector<btConvexHullShape*> shapes;
    std::vector<btDefaultMotionState*> motionStates;
    std::vector<btRigidBody*> rigidBodies;
    std::vector<double> mass;
    std::vector<btVector3> inertia;
    
    // Create vector to store points in
    std::vector<double> vtx;
    
    // Define loops stop flags
    bool simulationStopFlag = true;
    bool stackingStopFlag = true;
    int i = 0;     // Particle counter
    while(stackingStopFlag)
    {
        std::cout << "Creating cell " << i << std::endl;
        // Generate particle using voro++ funciton
        particleGen(v);
        // Generate convex hull object for bullet
        btConvexHullShape* hullShape = new btConvexHullShape();
        bulletHullShape(*hullShape, vtx);
        shapes.push_back(hullShape);
        // Generate convex hull motion state
        btDefaultMotionState* hullMotionState = new btDefaultMotionState();
        bulletHullMotionState(*hullMotionState);
        motionStates.push_back(hullMotionState);
        // Calculate convex hull mass
        double hullMass = v.volume() * rho;
        mass.push_back(hullMass);
        // Calculate convex hull object inertia
        btVector3 hullInertia(0,0,0);
        hullShape->calculateLocalInertia(mass[i], hullInertia);
        inertia.push_back(hullInertia);
        btRigidBody::btRigidBodyConstructionInfo
        hullRigidBodyCI
        (
         mass[i],
         motionStates[i],
         shapes[i],
         inertia[i]
         );
        // Set body friction
        hullRigidBodyCI.m_friction = friction;
        // Generate rigid body
        btRigidBody* hullRigidBody = new btRigidBody(hullRigidBodyCI);
        rigidBodies.push_back(hullRigidBody);
        // Add object to the world
        dynamicsWorld.addRigidBody(hullRigidBody);
        
        // SIMULATE
        hullC = {0.0, 0.0, 0.0};
        hullC0 = rigidBodies[i]->getCenterOfMassPosition();
        hullQuant = rigidBodies[i]->getOrientation();
        quaternionToEulerianAngle(hullQuant, hullRot);
        std::cout<<"hullC0 "  <<hullC0[0]<<" "<<hullC0[1]<< " "<<hullC0[2]<<std::endl;
        std::cout<<"hullRot0 "<<hullRot0[0]<<" "<<hullRot0[1]<<" "<<hullRot0[2]<<std::endl;
        // Simulate the world untill object stops moving
        // While stopFlag isn't raised run simulation
        while (simulationStopFlag)
        {
            // Run bullet world simulation
            dynamicsWorld.stepSimulation(1 / hz, 1 / hz);
            // Check if object is still moving.
            // Check if each coordinate movement distance by simulation step
            // is smaller then trashold given by user
            double xChange = std::fabs(rigidBodies[i]->getCenterOfMassPosition().getX() - hullC[0]);
            double yChange = std::fabs(rigidBodies[i]->getCenterOfMassPosition().getY() - hullC[1]);
            double zChange = std::fabs(rigidBodies[i]->getCenterOfMassPosition().getZ() - hullC[2]);
            if (
                xChange <= trashold &&
                yChange <= trashold &&
                zChange <= trashold
                )
            {
                // Get the object rotation and stop simulation
                hullQuant = rigidBodies[i]->getOrientation();
                break;
            }
            // Get particle position after simulation step
            hullC = rigidBodies[i]->getCenterOfMassPosition();
        }
        
        // Transform quaterinion angle to eulerian angle and print out position and angle
        quaternionToEulerianAngle(hullQuant, hullRot);
        std::cout<<"hullC "  <<hullC[0]<<" "<<hullC[1]<< " "<<hullC[2]<<std::endl;
        std::cout<<"hullRot "<<hullRot[0]<<" "<<hullRot[1]<<" "<<hullRot[2]<<std::endl;

        // Particle counter
        ++i;
        if (i == 50)
        {
            stackingStopFlag = false;
        }
 }
In that code some parameters (as density or simulation frequency hz) are given in additional user parameters script. Functions that define position are given in another script. Some of them are:

Code: Select all

// This function converts quaternion to eulerian
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
static void quaternionToEulerianAngle(const btQuaternion& q, btVector3& rotE)
{
    // btQuaterinion [x, y, z, w]
    double ysqr = q[1] * q[1];
    
    // roll (x-axis rotation)
    double t0 = +2.0 * (q[3] * q[0] + q[1] * q[2]);
    double t1 = +1.0 - 2.0 * (q[0] * q[0] + ysqr);
    rotE[0] = std::atan2(t0, t1);
    
    // pitch (y-axis rotation)
    double t2 = +2.0 * (q[3] * q[1] - q[2] * q[0]);
    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;
    rotE[1] = std::asin(t2);
    
    // yaw (z-axis rotation)
    double t3 = +2.0 * (q[3] * q[2] + q[0] * q[1]);
    double t4 = +1.0 - 2.0 * (ysqr + q[2] * q[2]);
    rotE[2] = std::atan2(t3, t4);
}

// This function generates bullets convex hull shape from voro's vertices
void bulletHullShape(btConvexHullShape& hullShape, const std::vector<double> vertices)
{
    // Loop trough new array of point coordinates
    for (int i = 0; i < vertices.size() / 3; ++i)
    {
        // Create a bullet points
        btVector3 btVtx
        (
         vertices[i * 3 ],
         vertices[i * 3 + 1],
         vertices[i * 3 + 2]
         );
        // Add point to hull
        hullShape.addPoint(btVtx);
    }
}

// This function generates convex hull motion state (positions and rotations)
void bulletHullMotionState(btDefaultMotionState& motionState)
{
    // Create motion state with random positions inside box bounds
    motionState = btDefaultMotionState
    (
     btTransform
     (
      btQuaternion(0, 0, 0, 1),
      btVector3
      (
        rnd(sphereDiaMax / 2, bulletBoxX - sphereDiaMax / 2),
        bulletBoxY,
        rnd(sphereDiaMax / 2, bulletBoxZ - sphereDiaMax / 2)
      )
     )
    );
}
Thank you all for help if you can supply some :)

Kruno


EDIT:
There was a mistake (I have just found it). I forgot to migrate vertex coordinates from voro object to vtx vector:

Code: Select all

        // Get point coordinates
        v.vertices(vtx);
This little piece goes after this piece of code (start of the particle generation loop)

Code: Select all

while(stackingStopFlag)
    {
        std::cout << "Creating cell " << i << std::endl;
        // Generate particle using voro++ funciton
        particleGen(v);

        // Get point coordinates
        v.vertices(vtx);
Output I get is:
Creating cell 0
hullC0 5.07536 13 5.24192
hullRot0 0 0 0
hullC 3.90271 0.0399999 6.47484
hullRot 0.785399 -0.921222 3.14159
Creating cell 1
hullC0 4.42766 13 5.55003
hullRot0 0 0 0
hullC 4.50988 0.103556 5.58856
hullRot 0.746631 -0.0239968 -0.0668973
Creating cell 2
hullC0 6.45497 13 2.73281
hullRot0 0 0 0
hullC 6.42421 0.0399999 2.66157
hullRot 0.785398 0.159969 -4.52816e-08
which seems reasonable but it seems that older particles are vanishing.
Kruno
Posts: 2
Joined: Fri Mar 17, 2017 6:53 pm

Re: Stacking of convex hull shapes

Post by Kruno »

Update (if it helps someone):
Unfortunately I haven't found the solution for running and stopping simulation in a loop but I managed to make my simulation by generating Xcode projects and editing BasicExample.
Now I can even visualise it and it looks awesome.
Image
Post Reply