void ompl::RNG::uniformNormalVector(unsigned int n, double value[])
{
// Create a uBLAS-vector-view of the C-style array without copying data
SphericalData::container_type_t rVector(n, value);
// Generate a random value, the variate_generator is returning a shallow_array_adaptor, which will modify the value array:
rVector = sphericalDataPtr_->generate(n);
}
/*
* Function which lets all the bodies added to this world collide with each other, calculates translations impulses.
* And the end we step all bodies forward in time by dt
*/
void rWorld::CollideAndStep( rReal dt)
{
// Update bodies
for(unsigned int i = 0; i < StateArray.size(); i++)
{
// If not a static body
if( StateArray[i]->InvMass > 0)
{
// Let the body do a step with the time and current gravity
StateArray[i]->Step( dt, Gravity);
StateArray[i]->CalculateMesh();
}
}
// Clear collisions from last time
CollisionArray.clear();
// Clear CollisionPoints
#ifdef R_SAVE_COLLISIONPOINTS
CollisionPoints.clear();
#endif
// Overlapping Solving Code. Inner loop for how many iterations requested to solve any errors
for(int iterations = 0; iterations < ERI; iterations++)
{
// If we are at the last iteration, save collisions
// TODO fix this properly. Always add collisions but never duplicate
bool push = false;
if (iterations == 0)
push = true;
// Two loops to let everybody check with each other
for( unsigned int i = 0; i < StateArray.size(); i++)
{
for( unsigned int j = i+1; j < StateArray.size(); j++)
{
// Sum of both bodies INV mass
rReal MassRatio = StateArray[i]->InvMass + StateArray[j]->InvMass;
// Check if a body is movable
if (MassRatio > 0)
{
// Create a collision package between the 2 bodies in line to be tested
rColinfo CollisionInfo( StateArray[i], StateArray[j]);
// Check if we have a collision by Separating Axis Test, if so store information in the collision package
if(::Collide( CollisionInfo))
{
// Are we overlapping?
if( CollisionInfo.Depth > EPSILON)
{
//Save required translations to solve any errors
StateArray[i]->Force_T += CollisionInfo.Force * (StateArray[i]->InvMass / MassRatio) * ERP;
StateArray[j]->Force_T -= CollisionInfo.Force * (StateArray[j]->InvMass / MassRatio) * ERP;
}
// Should we push?
if ( push)
CollisionArray.push_back( CollisionInfo);
}
}
}
}
// Apply any required translations as requested above
for( unsigned int i = 0; i < StateArray.size(); i++)
{
rBody* body = StateArray[i];
// If there is a
if( body->Force_T.LengthSquared() > 0)
{
body->Position += body->Force_T;
body->CalculateMeshTranslation( body->Force_T);
body->Force_T = rVector( 0, 0);
}
}
}
// Get collision points and process them
for( unsigned int i = 0; i < CollisionArray.size(); i++)
{
// Get collision point
rVector CollisionPoint = ::CollisionPoint( CollisionArray[i]);
// Add current collision point to the array
#ifdef R_SAVE_COLLISIONPOINTS
CollisionPoints.push_back( CollisionPoint);
#endif
// Process the collisionpoint
ProcessCollisionPoint( CollisionArray[i].getBody1(),
CollisionArray[i].getBody2(),
CollisionPoint,
CollisionArray[i].Force,
dt);
}
}