// This works ok at the moment. Need an effective way of determining if the rope is under an object
void tgBulletContactSpringCable::updateCollisionObject()
{
#ifndef BT_NO_PROFILE
BT_PROFILE("updateCollisionObject");
#endif //BT_NO_PROFILE
btDispatcher* m_dispatcher = tgBulletUtil::worldToDynamicsWorld(m_world).getDispatcher();
btBroadphaseInterface* const m_overlappingPairCache = tgBulletUtil::worldToDynamicsWorld(m_world).getBroadphase();
// Clear the existing child shapes
btCompoundShape* m_compoundShape = tgCast::cast<btCollisionShape, btCompoundShape> (m_ghostObject->getCollisionShape());
clearCompoundShape(m_compoundShape);
btVector3 maxes(anchor2->getWorldPosition());
btVector3 mins(anchor1->getWorldPosition());
std::size_t n = m_anchors.size();
for (std::size_t i = 0; i < n; i++)
{
btVector3 worldPos = m_anchors[i]->getWorldPosition();
for (std::size_t j = 0; j < 3; j++)
{
if (worldPos[j] > maxes[j])
{
maxes[j] = worldPos[j];
}
if (worldPos[j] < mins[j])
{
mins[j] = worldPos[j];
}
}
}
btVector3 center = (maxes + mins)/2.0;
btVector3 from = anchor1->getWorldPosition();
btVector3 to = anchor2->getWorldPosition();
for (std::size_t i = 0; i < n-1; i++)
{
btVector3 pos1 = m_anchors[i]->getWorldPosition();
btVector3 pos2 = m_anchors[i+1]->getWorldPosition();
// Children handles the orientation data
btTransform t = tgUtil::getTransform(pos2, pos1);
t.setOrigin(t.getOrigin() - center);
btScalar length = (pos2 - pos1).length() / 2.0;
/// @todo - seriously examine box vs cylinder shapes
btCylinderShape* box = new btCylinderShape(btVector3(m_thickness, length, m_thickness));
m_compoundShape->addChildShape(t, box);
}
// Default margin is 0.04, so larger than default thickness. Behavior is better with larger margin
//m_compoundShape->setMargin(m_thickness);
btTransform transform;
transform.setOrigin(center);
transform.setRotation(btQuaternion::getIdentity());
m_ghostObject->setCollisionShape (m_compoundShape);
m_ghostObject->setWorldTransform(transform);
// Delete the existing contacts in bullet to prevent sticking - may exacerbate problems with rotations
m_overlappingPairCache->getOverlappingPairCache()->cleanProxyFromPairs(m_ghostObject->getBroadphaseHandle(),m_dispatcher);
}
bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
const btConvexShape* convexA,const btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btVector3& pa, btVector3& pb,
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
)
{
(void)stackAlloc;
(void)v;
bool check2d= convexA->isConvex2d() && convexB->isConvex2d();
struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
{
btIntermediateResult():m_hasResult(false)
{
}
btVector3 m_normalOnBInWorld;
btVector3 m_pointInWorld;
btScalar m_depth;
bool m_hasResult;
virtual void setShapeIdentifiersA(int partId0,int index0)
{
(void)partId0;
(void)index0;
}
virtual void setShapeIdentifiersB(int partId1,int index1)
{
(void)partId1;
(void)index1;
}
void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
m_normalOnBInWorld = normalOnBInWorld;
m_pointInWorld = pointInWorld;
m_depth = depth;
m_hasResult = true;
}
};
//just take fixed number of orientation, and sample the penetration depth in that direction
btScalar minProj = btScalar(BT_LARGE_FLOAT);
btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
btVector3 minA,minB;
btVector3 seperatingAxisInA,seperatingAxisInB;
btVector3 pInA,qInB,pWorld,qWorld,w;
#ifndef __SPU__
#define USE_BATCHED_SUPPORT 1
#endif
#ifdef USE_BATCHED_SUPPORT
btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i;
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (i=0;i<numSampleDirections;i++)
{
btVector3 norm = sPenetrationDirections[i];
seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ;
seperatingAxisInBBatch[i] = norm * transB.getBasis() ;
}
{
int numPDA = convexA->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
{
btVector3 norm;
convexA->getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
}
{
int numPDB = convexB->getNumPreferredPenetrationDirections();
if (numPDB)
{
for (int i=0;i<numPDB;i++)
{
btVector3 norm;
convexB->getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm;
seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
numSampleDirections++;
}
}
}
convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections);
convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections);
for (i=0;i<numSampleDirections;i++)
{
btVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm[2] = 0.f;
}
if (norm.length2()>0.01)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = supportVerticesABatch[i];
qInB = supportVerticesBBatch[i];
pWorld = transA(pInA);
qWorld = transB(qInB);
if (check2d)
{
pWorld[2] = 0.f;
qWorld[2] = 0.f;
}
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
#ifndef __SPU__
{
int numPDA = convexA->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
{
btVector3 norm;
convexA->getPreferredPenetrationDirection(i,norm);
norm = transA.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB->getNumPreferredPenetrationDirections();
if (numPDB)
{
for (int i=0;i<numPDB;i++)
{
btVector3 norm;
convexB->getPreferredPenetrationDirection(i,norm);
norm = transB.getBasis() * norm;
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
#endif // __SPU__
for (int i=0;i<numSampleDirections;i++)
{
const btVector3& norm = sPenetrationDirections[i];
seperatingAxisInA = (-norm)* transA.getBasis();
seperatingAxisInB = norm* transB.getBasis();
pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
pWorld = transA(pInA);
qWorld = transB(qInB);
w = qWorld - pWorld;
btScalar delta = norm.dot(w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm*convexA->getMarginNonVirtual();
minB -= minNorm*convexB->getMarginNonVirtual();
//no penetration
if (minProj < btScalar(0.))
return false;
btScalar extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
//#define DEBUG_DRAW 1
#ifdef DEBUG_DRAW
if (debugDraw)
{
btVector3 color(0,1,0);
debugDraw->drawLine(minA,minB,color);
color = btVector3 (1,1,1);
btVector3 vec = minB-minA;
btScalar prj2 = minNorm.dot(vec);
debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
}
#endif //DEBUG_DRAW
btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
btScalar offsetDist = minProj;
btVector3 offset = minNorm * offsetDist;
btGjkPairDetector::ClosestPointInput input;
btVector3 newOrg = transA.getOrigin() + offset;
btTransform displacedTrans = transA;
displacedTrans.setOrigin(newOrg);
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj;
btIntermediateResult res;
gjkdet.setCachedSeperatingAxis(-minNorm);
gjkdet.getClosestPoints(input,res,debugDraw);
btScalar correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
btScalar penetration_relaxation= btScalar(1.);
minNorm*=penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#ifdef DEBUG_DRAW
if (debugDraw)
{
btVector3 color(1,0,0);
debugDraw->drawLine(pa,pb,color);
}
#endif//DEBUG_DRAW
}
return res.m_hasResult;
}
void tgBulletContactSpringCable::calculateAndApplyForce(double dt)
{
#ifndef BT_NO_PROFILE
BT_PROFILE("calculateAndApplyForce");
#endif //BT_NO_PROFILE
const double tension = getTension();
const double currLength = getActualLength();
const double deltaStretch = currLength - m_prevLength;
m_velocity = deltaStretch / dt;
m_damping = m_dampingCoefficient * m_velocity;
if (btFabs(tension) * 1.0 < btFabs(m_damping))
{
m_damping =
(m_damping > 0.0 ? tension * 1.0 : -tension * 1.0);
}
const double magnitude = tension + m_damping;
// Apply forces
std::size_t n = m_anchors.size();
for (std::size_t i = 0; i < n; i++)
{
btVector3 force = btVector3 (0.0, 0.0, 0.0);
if (i == 0)
{
btVector3 direction = m_anchors[i + 1]->getWorldPosition() - m_anchors[i]->getWorldPosition();
force = direction.normalize() * magnitude;
}
// Will likely only be true for the last anchor
else if (m_anchors[i]->sliding == false)
{
btVector3 direction = m_anchors[i]->getWorldPosition() - m_anchors[i - 1]->getWorldPosition();
force = -direction.normalize() * magnitude;
}
else if (i < n - 1)
{
// Already normalized
btVector3 direction = m_anchors[i]->getContactNormal();
// Get normal to string
btVector3 back = m_anchors[i - 1]->getWorldPosition();
btVector3 current = m_anchors[i]->getWorldPosition();
btVector3 forward = m_anchors[i + 1]->getWorldPosition();
btVector3 first = (forward - current);
btVector3 second = (back - current);
btVector3 forceDir = first.normalize() + second.normalize();
// For sliding anchors, just figuring out directions for now
force = magnitude * forceDir;
}
else
{
throw std::runtime_error("tgBulletContactSpringCable: First or last anchor is a sliding constraint!!");
}
m_anchors[i]->force = force;
}
btVector3 totalForce(0.0, 0.0, 0.0);
for (std::size_t i = 0; i < n; i++)
{
btRigidBody* body = m_anchors[i]->attachedBody;
btVector3 contactPoint = m_anchors[i]->getRelativePosition();
body->activate();
totalForce += m_anchors[i]->force;
btVector3 impulse = m_anchors[i]->force* dt;
body->applyImpulse(impulse, contactPoint);
}
if (!totalForce.fuzzyZero())
{
std::cout << "Total Force Error! " << totalForce << std::endl;
throw std::runtime_error("Total force did not sum to zero!");
}
// Finished calculating, so can store things
m_prevLength = currLength;
}
void PhysicsObject::setScale(float amount)
{
btVector3 scaleAmount = btVector3(amount,amount,amount);
this->collisionObject->getCollisionShape()->setLocalScaling(scaleAmount);
this->updateTransformationMatrix();
}
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#define NUM_UNITSPHERE_POINTS 42
static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
{
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
osg::Node* generateGroundPlane( const osg::Vec4& plane, btDynamicsWorld* bulletWorld, btRigidBody** rb, const short group, const short mask )
{
osg::Vec3 n(plane.x(),plane.y(),plane.z());
n.normalize();
float d (plane.w());
osg::Vec3 v (1.f,0,0);
// TBD consider using osg::Vec3::operator^ for cross product: (v^n)
osg::Vec3 u1 = v -n*(v.x()*n.x() +v.y()*n.y() + v.z()*n.z());
osg::Vec3 u2;
if (u1.length()==0){
u1 = osg::Vec3(0.f,1.f,0.f);
u2 = osg::Vec3(0.f,0.f,1.f);
}
else{
u1.normalize();
u2 = n^u1;
u2.normalize();
}
osg::Vec3 p = n * d;
// TBD use new stuff in Shapes.
const btVector3 planeNormal( plane.x(), plane.y(), plane.z() );
btCollisionShape* groundShape = new btStaticPlaneShape( planeNormal, plane.w() );
btRigidBody::btRigidBodyConstructionInfo rbInfo( 0., NULL, groundShape, btVector3(0,0,0) );
btRigidBody* ground = new btRigidBody(rbInfo);
btDiscreteDynamicsWorld* dw = dynamic_cast< btDiscreteDynamicsWorld* >( bulletWorld );
if( ( dw != NULL ) && ( ( group != 0 ) || ( mask != 0 ) ) )
dw->addRigidBody( ground, group, mask );
else
bulletWorld->addRigidBody( ground );
if( rb != NULL )
*rb = ground;
osg::ref_ptr< osg::Geode > groundPlane = new osg::Geode;
osg::Geometry* groundGeom = new osg::Geometry;
groundPlane->addDrawable(groundGeom);
osg::ref_ptr<osg::Vec3Array> vertarray = new osg::Vec3Array;
groundGeom->setVertexArray( vertarray.get() );
int width(30);
osg::Vec3 point;
const int nVerts( 4*width+2 );
for(int i = -width; i < width; i++)
{
for(int j = -width; j < width+1; j ++)
{
vertarray->push_back(p + u1*i + u2*j);
vertarray->push_back(p + u1*(i+1) + u2*j);
}
groundGeom->addPrimitiveSet( new osg::DrawArrays(
osg::PrimitiveSet::TRIANGLE_STRIP, (i+width)*nVerts, nVerts ) );
}
osg::ref_ptr<osg::Vec3Array> norm = new osg::Vec3Array;
groundGeom->setNormalArray( norm.get() );
norm->push_back( n );
groundGeom->setNormalBinding( osg::Geometry::BIND_OVERALL );
osg::ref_ptr<osg::Vec4Array> c = new osg::Vec4Array;
groundGeom->setColorArray( c.get() );
c->push_back( osg::Vec4( 1.f, 1.f, 1.f, 1.f ) );
groundGeom->setColorBinding( osg::Geometry::BIND_OVERALL );
return( groundPlane.release() );
}
plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
{
void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
}
void btPolyhedralConvexAabbCachingShape::recalcLocalAabb()
{
m_isLocalAabbValid = true;
#if 1
static const btVector3 _directions[] =
{
btVector3( 1., 0., 0.),
btVector3( 0., 1., 0.),
btVector3( 0., 0., 1.),
btVector3( -1., 0., 0.),
btVector3( 0., -1., 0.),
btVector3( 0., 0., -1.)
};
btVector3 _supporting[] =
{
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.),
btVector3( 0., 0., 0.)
};
batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
for ( int i = 0; i < 3; ++i )
{
m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin;
m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin;
}
#else
for (int i=0;i<3;i++)
{
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
vec[i] = btScalar(1.);
btVector3 tmp = localGetSupportingVertex(vec);
m_localAabbMax[i] = tmp[i]+m_collisionMargin;
vec[i] = btScalar(-1.);
tmp = localGetSupportingVertex(vec);
m_localAabbMin[i] = tmp[i]-m_collisionMargin;
}
#endif
}
void RigidBody::UpdateMass()
{
if (!body_ || !enableMassUpdate_)
return;
btTransform principal;
principal.setRotation(btQuaternion::getIdentity());
principal.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
// Calculate center of mass shift from all the collision shapes
unsigned numShapes = (unsigned)compoundShape_->getNumChildShapes();
if (numShapes)
{
PODVector<float> masses(numShapes);
for (unsigned i = 0; i < numShapes; ++i)
{
// The actual mass does not matter, divide evenly between child shapes
masses[i] = 1.0f;
}
btVector3 inertia(0.0f, 0.0f, 0.0f);
compoundShape_->calculatePrincipalAxisTransform(&masses[0], principal, inertia);
}
// Add child shapes to shifted compound shape with adjusted offset
while (shiftedCompoundShape_->getNumChildShapes())
shiftedCompoundShape_->removeChildShapeByIndex(shiftedCompoundShape_->getNumChildShapes() - 1);
for (unsigned i = 0; i < numShapes; ++i)
{
btTransform adjusted = compoundShape_->getChildTransform(i);
adjusted.setOrigin(adjusted.getOrigin() - principal.getOrigin());
shiftedCompoundShape_->addChildShape(adjusted, compoundShape_->getChildShape(i));
}
// If shifted compound shape has only one child with no offset/rotation, use the child shape
// directly as the rigid body collision shape for better collision detection performance
bool useCompound = !numShapes || numShapes > 1;
if (!useCompound)
{
const btTransform& childTransform = shiftedCompoundShape_->getChildTransform(0);
if (!ToVector3(childTransform.getOrigin()).Equals(Vector3::ZERO) ||
!ToQuaternion(childTransform.getRotation()).Equals(Quaternion::IDENTITY))
useCompound = true;
}
body_->setCollisionShape(useCompound ? shiftedCompoundShape_ : shiftedCompoundShape_->getChildShape(0));
// If we have one shape and this is a triangle mesh, we use a custom material callback in order to adjust internal edges
if (!useCompound && body_->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE &&
physicsWorld_->GetInternalEdge())
body_->setCollisionFlags(body_->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
else
body_->setCollisionFlags(body_->getCollisionFlags() & ~btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
// Reapply rigid body position with new center of mass shift
Vector3 oldPosition = GetPosition();
centerOfMass_ = ToVector3(principal.getOrigin());
SetPosition(oldPosition);
// Calculate final inertia
btVector3 localInertia(0.0f, 0.0f, 0.0f);
if (mass_ > 0.0f)
shiftedCompoundShape_->calculateLocalInertia(mass_, localInertia);
body_->setMassProps(mass_, localInertia);
body_->updateInertiaTensor();
// Reapply constraint positions for new center of mass shift
if (node_)
{
for (PODVector<Constraint*>::Iterator i = constraints_.Begin(); i != constraints_.End(); ++i)
(*i)->ApplyFrames();
}
}
workshopScene::workshopScene(std::string path_)
:mouseRayCallback(btVector3(0, 0, 0), btVector3(0, 0, 0)),
axisResult(btVector3(0, 0, 0), btVector3(0, 0, 0))
{
path = path_;
DIR *dir;
dirent *ent;
gWorld = new world();
gWorld->btWorld->setGravity(btVector3(0, 0, 0));
mouseConstraint = 0;
std::stringstream ss;
ss << path << "data/plane_10x10.bsm";
gWorld->addObject(new physObj(0, btVector3(0, -1, 0), new btStaticPlaneShape(btVector3(0, 1, 0), 1), new model(ss.str())));
ss.str("");
ss << path << "assemblies";
dir = opendir(ss.str().c_str());
if (dir == NULL)
{
std::cout << "Could not open assemblies directory.\n";
return;
}
int i = 0;
while ((ent = readdir(dir)) != NULL)
{
if (ent->d_type == DT_DIR && ++i > 2) //discard "." and ".."
{
partnames.push_back(ent->d_name);
}
}
closedir(dir);
for (unsigned int i = 0; i < partnames.size(); i++)
{
std::cout << "Part: " << partnames[i] << "\n";
std::stringstream ss;
ss << path << "assemblies/" << partnames[i] << "/thumb.tga";
std::cout << ss.str() << "\n";
GLFWimage img;
if (glfwReadImage(ss.str().c_str(), &img, 0))
{
thumbnails.push_back(makeTexture(img));
glfwFreeImage(&img);
}
else
{
thumbnails.push_back(0);
std::cout << "Could not load thumbnail for " << partnames[i] << "\n";
}
}
cursor = textureFromFile("cursor.tga");
panel = ninePatch(textureFromFile("9patch.tga"));
button = ninePatch(textureFromFile("button.tga"));
bubble = ninePatch(textureFromFile("bubble.tga"), 8, 16, 12, 8, 0.25, 0.5, 0.375, 0.75);
tooltextures.push_back(textureFromFile("drag.tga"));
tooltextures.push_back(textureFromFile("axis.tga"));
tooltextures.push_back(textureFromFile("delete.tga"));
font = textureFromFile("font.tga");
cursorx = 0;
cursory = 0;
mousevelx = 0;
mousevely = 0;
mouseWasCaptured = false;
camera.position = btVector3(0, 0, 10);
camera.pitch = 0;
camera.yaw = 0;
camera.orientationFromAngles();
selectedItem = -1;
selectedTool = -1;
static unsigned char fontpixels[STB_SOMEFONT_BITMAP_HEIGHT][STB_SOMEFONT_BITMAP_WIDTH];
STB_SOMEFONT_CREATE(fontdata, fontpixels, STB_SOMEFONT_BITMAP_HEIGHT);
glGenTextures(1, &stbfont);
glBindTexture(GL_TEXTURE_2D, stbfont);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_WRAP_BORDER);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_WRAP_BORDER);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, STB_SOMEFONT_BITMAP_HEIGHT, STB_SOMEFONT_BITMAP_WIDTH, 0, GL_ALPHA, GL_UNSIGNED_BYTE, fontpixels);
}
void workshopScene::update(sceneInfo &info)
{
gWorld->btWorld->stepSimulation(1.f/60.f);
mousevelx = mousevelx * 0.95f + info.dmousex * 0.2f;
mousevely = mousevely * 0.95f + info.dmousey * 0.2f;
if (info.keys.held.MouseM)
{
camera.pitch -= info.dmousey * 0.02;
camera.yaw -= info.dmousex * 0.02;
camera.orientationFromAngles();
}
sceneInfo::keyState &keys = info.keys;
if (!glfwGetKey('E'))
{
if (keys.held.W)
{
camera.position += camera.forward * 0.1;
}
else if (keys.held.S)
{
camera.position -= camera.forward * 0.1;
}
if (keys.held.D)
{
camera.position += camera.right * 0.1;
}
else if (keys.held.A)
{
camera.position -= camera.right * 0.1;
}
}
if (info.captureMouse)
{
if (mouseWasCaptured)
{
cursorx += info.dmousex;
cursory += info.dmousey;
if (cursorx < 0)
cursorx = 0;
if (cursorx > info.width)
cursorx = info.width;
if (cursory < 0)
cursory = 0;
if (cursory > info.height)
cursory = info.height;
}
else
{
cursorx = info.lastmousex;
cursory = info.lastmousey;
}
}
else if (mouseWasCaptured)
{
glfwSetMousePos(cursorx, cursory);
}
mouseWasCaptured = info.captureMouse;
if (info.keys.held.space && selectedItem < partnames.size())
{
int initialcount = gWorld->objects.size();
loadAssembly(partnames[selectedItem], btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 10, 0)), path, gWorld);
for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
}
if (cursorx < UI_SIZE)
{
if (keys.newPress.MouseL)
{
selectedItem = (cursory - 4) / (UI_SIZE + 8);
selectedTool = -1;
}
}
else if (cursory > info.height - UI_SMALL - 16)
{
if (keys.newPress.MouseL)
{
selectedTool = (cursorx - UI_SIZE - 16 - 8) / (UI_SMALL + 8);
selectedItem = -1;
}
}
else
{
mouseRayDir = camera.forward + camera.right * (cursorx - info.width / 2) / (float)info.height * 2 - camera.up * (cursory - info.height / 2) / (float)info.height * 2;
btVector3 raystart = camera.position;
btVector3 rayend = raystart + mouseRayDir * 100;
mouseRayCallback = btCollisionWorld::ClosestRayResultCallback(raystart, rayend);
gWorld->btWorld->rayTest(raystart, rayend, mouseRayCallback);
if (keys.newPress.MouseL && mouseRayCallback.hasHit())
{
mouseHeldBody = (btRigidBody*)mouseRayCallback.m_collisionObject;
if (selectedItem >= 0)
{
if (selectedItem < partnames.size())
{
int initialcount = gWorld->objects.size();
btTransform trans(btQuaternion(0, 0, 0, 1), mouseRayCallback.m_hitPointWorld + mouseRayCallback.m_hitNormalWorld * 1.f);
loadAssembly(partnames[selectedItem], trans, path, gWorld);
for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
}
}
else if (selectedTool == 0)
{
mousePerpDist = camera.forward.dot(mouseRayCallback.m_hitPointWorld - raystart);
mouseHeldBody->setDamping(0.995, 0.98);
mouseHeldBody->activate();
btVector3 localPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld;
mouseConstraint = new btGeneric6DofConstraint(*mouseHeldBody, btTransform(btQuaternion(0, 0, 0, 1), localPivot), false);
if (glfwGetKey('E'))
{
mouseConstraint->setAngularLowerLimit(btVector3(0, 0, 0));
mouseConstraint->setAngularUpperLimit(btVector3(0, 0, 0));
}
gWorld->btWorld->addConstraint(mouseConstraint);
}
else if (selectedTool == 1)
{
if (!mouseHeldBody->isStaticObject())
{
if (!axisHasFirst)
{
axisHasFirst = true;
axisResult = mouseRayCallback;
axisFirstPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * axisResult.m_hitPointWorld;
axisFirstNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * axisResult.m_hitNormalWorld;
std::cout << "First point for axis.\n";
}
else
{
btVector3 axisSecondNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * mouseRayCallback.m_hitNormalWorld;
btVector3 axisSecondPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld + axisSecondNormal * 0.05;
gWorld->addConstraint(new btHingeConstraint(*(btRigidBody*)axisResult.m_collisionObject, *mouseHeldBody, axisFirstPivot, axisSecondPivot, -axisFirstNormal, axisSecondNormal));
mouseHeldBody->activate();
axisHasFirst = false;
}
}
}
else if (selectedTool == 2)
{
btRigidBody *body = (btRigidBody*)mouseRayCallback.m_collisionObject;
if (!body->isStaticObject())
gWorld->removeBody(body);
}
}
}
if (mouseConstraint)
{
if (keys.held.MouseL)
{
mousePerpDist *= pow(1.1, keys.dmouseWheel);
mouseConstraint->getFrameOffsetA().setOrigin(camera.position + mouseRayDir * mousePerpDist); // note that raydir is not unit length: it stretches from the camera to the near plane.
}
else
{
mouseHeldBody->setDamping(0.95, 0.96);
gWorld->btWorld->removeConstraint(mouseConstraint);
delete mouseConstraint;
mouseConstraint = 0;
}
if (glfwGetKey('E'))
{
btTransform invrotate;
mouseHeldBody->getMotionState()->getWorldTransform(invrotate);
invrotate = invrotate.inverse();
if (keys.held.W)
mouseHeldBody->applyImpulse(invrotate * camera.forward, invrotate * camera.up);
if (keys.held.S)
mouseHeldBody->applyImpulse(-camera.forward, camera.up);
if (keys.held.A)
mouseHeldBody->applyImpulse(-camera.right, camera.forward);
if (keys.held.D)
mouseHeldBody->applyImpulse(camera.right, camera.forward);
}
}
if (axisHasFirst && selectedTool != 1)
axisHasFirst = false;
}
void btRaycastVehicle::updateFriction(btScalar timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = getNumWheels();
if (!numWheel)
return;
m_forwardWS.resize(numWheel);
m_axle.resize(numWheel);
m_forwardImpulse.resize(numWheel);
m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i=0;i<getNumWheels();i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
numWheelsOnGround++;
m_sideImpulse[i] = btScalar(0.);
m_forwardImpulse[i] = btScalar(0.);
}
{
for (int i=0;i<getNumWheels();i++)
{
btWheelInfo& wheelInfo = m_wheelInfo[i];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
if (groundObject)
{
const btTransform& wheelTrans = getWheelTransformWS( i );
btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
m_axle[i] = btVector3(
wheelBasis0[0][m_indexRightAxis],
wheelBasis0[1][m_indexRightAxis],
wheelBasis0[2][m_indexRightAxis]);
const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
btScalar proj = m_axle[i].dot(surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i] = m_axle[i].normalize();
m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
//m_forwardWS[i] = m_axle[i].cross( surfNormalWS );
m_forwardWS[i].normalize();
resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
*groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
m_sideImpulse[i] *= sideFrictionStiffness2;
}
}
}
btScalar sideFactor = btScalar(1.);
btScalar fwdFactor = 0.5;
bool sliding = false;
{
for (int wheel =0;wheel <getNumWheels();wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
btScalar rollingFriction = 0.f;
if (groundObject)
{
if (wheelInfo.m_engineForce != 0.f)
{
rollingFriction = wheelInfo.m_engineForce* timeStep;
} else
{
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = btScalar(0.);
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
if (groundObject)
{
m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
btScalar maximpSide = maximp;
btScalar maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
btScalar impulseSquared = (x*x + y*y);
if (impulseSquared > maximpSquared)
{
sliding = true;
btScalar factor = maximp / btSqrt(impulseSquared);
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0;wheel < getNumWheels(); wheel++)
{
if (m_sideImpulse[wheel] != btScalar(0.))
{
if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0;wheel<getNumWheels() ; wheel++)
{
btWheelInfo& wheelInfo = m_wheelInfo[wheel];
btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody->getCenterOfMassPosition();
if (m_forwardImpulse[wheel] != btScalar(0.))
{
m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
}
if (m_sideImpulse[wheel] != btScalar(0.))
{
class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject->getCenterOfMassPosition();
btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if defined ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform().getBasis().getColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody->applyImpulse(sideImp,rel_pos);
//apply friction impulse on the ground
groundObject->applyImpulse(-sideImp,rel_pos2);
}
}
}
}
btRigidBody& btActionInterface::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
return s_fixed;
}
void PhysicsObject::setScale(glm::vec3 vector)
{
btVector3 scaleAmount = btVector3(vector.x,vector.y,vector.z);
this->collisionObject->getCollisionShape()->setLocalScaling(scaleAmount);
this->updateTransformationMatrix();
}
btVector3 Vec3f::toBt() const {
return btVector3(x, y ,z);
}
void InternalEdgeDemo::initPhysics()
{
setTexturing(true);
setShadows(false);//true);
#define TRISIZE 10.f
gContactAddedCallback = CustomMaterialCombinerCallback;
#define USE_TRIMESH_SHAPE 1
#ifdef USE_TRIMESH_SHAPE
int vertStride = sizeof(btVector3);
int indexStride = 3*sizeof(int);
const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
gVertices = new btVector3[totalVerts];
gIndices = new int[totalTriangles*3];
int i;
setVertexPositions(waveheight,0.f);
//gVertices[1].setY(21.1);
//gVertices[1].setY(121.1);
gVertices[1].setY(.1f);
#ifdef ROTATE_GROUND
//gVertices[1].setY(-1.1);
#else
//gVertices[1].setY(0.1);
//gVertices[1].setY(-0.1);
//gVertices[1].setY(-20.1);
//gVertices[1].setY(-20);
#endif
int index=0;
for ( i=0;i<NUM_VERTS_X-1;i++)
{
for (int j=0;j<NUM_VERTS_Y-1;j++)
{
#ifdef SWAP_WINDING
#ifdef SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#ifdef SHIFT_INDICES
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
#ifdef TEST_INCONSISTENT_WINDING
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
bool useQuantizedAabbCompression = true;
//comment out the next line to read the BVH from disk (first run the demo once to create the BVH)
#define SERIALIZE_TO_DISK 1
#ifdef SERIALIZE_TO_DISK
btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000);
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
m_collisionShapes.push_back(trimeshShape);
///we can serialize the BVH data
void* buffer = 0;
int numBytes = trimeshShape->getOptimizedBvh()->calculateSerializeBufferSize();
buffer = btAlignedAlloc(numBytes,16);
bool swapEndian = false;
trimeshShape->getOptimizedBvh()->serialize(buffer,numBytes,swapEndian);
FILE* file = fopen("bvh.bin","wb");
fwrite(buffer,1,numBytes,file);
fclose(file);
btAlignedFree(buffer);
#else
trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,false);
char* fileName = "bvh.bin";
FILE* file = fopen(fileName,"rb");
int size=0;
btOptimizedBvh* bvh = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET)) { /* File operations denied? ok, just close and return failure */
printf("Error: cannot get filesize from %s\n", fileName);
exit(0);
} else
{
fseek(file, 0, SEEK_SET);
int buffersize = size+btOptimizedBvh::getAlignmentSerializationPadding();
void* buffer = btAlignedAlloc(buffersize,16);
int read = fread(buffer,1,size,file);
fclose(file);
bool swapEndian = false;
bvh = btOptimizedBvh::deSerializeInPlace(buffer,buffersize,swapEndian);
}
trimeshShape->setOptimizedBvh(bvh);
#endif
btCollisionShape* groundShape = trimeshShape;
btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(trimeshShape,triangleInfoMap);
#else
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_collisionShapes.push_back(groundShape);
#endif //USE_TRIMESH_SHAPE
m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
/*
m_dynamicsWorld->getSolverInfo().m_splitImpulse = true;
m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = 1e30f;
m_dynamicsWorld->getSolverInfo().m_maxErrorReduction = 1e30f;
m_dynamicsWorld->getSolverInfo().m_erp =1.f;
m_dynamicsWorld->getSolverInfo().m_erp2 = 1.f;
*/
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
float mass = 0.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-2,0));
btConvexHullShape* colShape = new btConvexHullShape();
for (int i=0;i<TaruVtxCount;i++)
{
btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
colShape->addPoint(vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
colShape->initializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld->getDispatchInfo().m_enableSatConvex=false;
m_collisionShapes.push_back(colShape);
{
for (int i=0;i<1;i++)
{
startTransform.setOrigin(btVector3(-10.f+i*3.f,2.2f+btScalar(i)*0.1f,-1.3f));
btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0,0,-1));
//body->setContactProcessingThreshold(0.f);
}
}
{
btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
colShape->initializePolyhedralFeatures();
m_collisionShapes.push_back(colShape);
startTransform.setOrigin(btVector3(-16.f+i*3.f,1.f+btScalar(i)*0.1f,-1.3f));
btRigidBody* body = localCreateRigidBody(10, startTransform,colShape);
body->setActivationState(DISABLE_DEACTIVATION);
body->setLinearVelocity(btVector3(0,0,-1));
}
startTransform.setIdentity();
#ifdef ROTATE_GROUND
btQuaternion orn(btVector3(0,0,1),SIMD_PI);
startTransform.setOrigin(btVector3(-20,0,0));
startTransform.setRotation(orn);
#endif //ROTATE_GROUND
staticBody = localCreateRigidBody(mass, startTransform,groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);//STATIC_OBJECT);
//enable custom material callback
staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
setDebugMode(btIDebugDraw::DBG_DrawText|btIDebugDraw::DBG_NoHelpText+btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btSetDebugDrawer(&gDebugDrawer);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
}
//--------------------------------------------------------------
void ofxBulletSoftBody::setNodePositionAt(size_t n, const glm::vec3& pos) {
_softBody->m_nodes.at(n).m_x = btVector3(pos.x, pos.y, pos.z);
}
void CollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
btVoronoiSimplexSolver sGjkSimplexSolver;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
//VECCOPY(pa, gjkOutput.m_pointInWorld);
//VECCOPY(pb, gjkOutput.m_pointInWorld);
//VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance);
printf("bullet: %10.10f\n", gjkOutput.m_distance); // = 0.24 => that's absolutely wrong!
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
// btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
//let it rotate
//tr[0].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
{
void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
}
void CollisionDemo::initPhysics()
{
m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
#ifdef CHECK_GENSHER_TRIANGLE_CASE
m_azi = 140.f;
#else
m_azi = 250.f;
#endif
m_ele = 25.f;
tr[0].setOrigin(btVector3(0.0013328250f,8.1363249f,7.0390840f));
tr[1].setOrigin(btVector3(0.00000000f,9.1262732f,2.0343180f));
//tr[0].setOrigin(btVector3(0,0,0));
//tr[1].setOrigin(btVector3(0,10,0));
btMatrix3x3 basisA;
basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f,
-0.00029313788f,0.99753088f,0.070228584f,
-0.00089153741f,-0.070228823f,0.99753052f);
btMatrix3x3 basisB;
basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f,
4.4865495e-018f,0.97979438f,0.20000751f,
4.4410586e-017f,-0.20000751f,0.97979438f);
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
#ifdef CHECK_GENSHER_TRIANGLE_CASE
tr[0].setIdentity();
tr[1].setIdentity();
#endif //CHECK_GENSHER_TRIANGLE_CASE
btVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f);
btVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f);
#ifndef CHECK_GENSHER_TRIANGLE_CASE
btBoxShape* boxA = new btBoxShape(boxHalfExtentsA);
btBoxShape* boxB = new btBoxShape(boxHalfExtentsB);
#endif
float p1[3], p2[3], p3[3], q1[3],q2[3], q3[3];
p1[0] = -3.9242966175;
p1[1] = -0.5582823175;
p1[2] = 2.0101921558;
p2[0] = -3.6731941700;
p2[1] = -0.5604774356;
p2[2] = 2.00301921558;
p3[0] = -3.6698703766;
p3[1] = -0.3097069263;
p3[2] = 2.0073683262;
q1[0] = -2.6317186356;
q1[1] = -1.0000005960;
q1[2] = 1.9999998808;
q2[0] = -2.6317174435;
q2[1] = 0.9999994636;
q2[2] = 1.9999998808;
q3[0] = -4.6317176819;
q3[1] = 1.0f;
q3[2] = 1.9999998808;
btTriangleShape* trishapeA = new btTriangleShape(btVector3(p1[0], p1[1], p1[2]), btVector3(p2[0], p2[1], p2[2]), btVector3(p3[0], p3[1], p3[2]));
trishapeA->setMargin(0.001f);
btTriangleShape* trishapeB = new btTriangleShape(btVector3(q1[0], q1[1], q1[2]), btVector3(q2[0], q2[1], q2[2]), btVector3(q3[0], q3[1], q3[2]));
trishapeB->setMargin(0.001f);
#ifdef CHECK_GENSHER_TRIANGLE_CASE
shapePtr[0] = trishapeA;
shapePtr[1] = trishapeB;
#else
shapePtr[0] = boxA;
shapePtr[1] = boxB;
#endif
}
double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
{
btVector3 vp(p1[0], p1[1], p1[2]);
btTriangleShape trishapeA(vp,
btVector3(p2[0], p2[1], p2[2]),
btVector3(p3[0], p3[1], p3[2]));
trishapeA.setMargin(0.000001f);
btVector3 vq(q1[0], q1[1], q1[2]);
btTriangleShape trishapeB(vq,
btVector3(q2[0], q2[1], q2[2]),
btVector3(q3[0], q3[1], q3[2]));
trishapeB.setMargin(0.000001f);
// btVoronoiSimplexSolver sGjkSimplexSolver;
// btGjkEpaPenetrationDepthSolver penSolverPtr;
static btSimplexSolverInterface sGjkSimplexSolver;
sGjkSimplexSolver.reset();
static btGjkEpaPenetrationDepthSolver Solver0;
static btMinkowskiPenetrationDepthSolver Solver1;
btConvexPenetrationDepthSolver* Solver = NULL;
Solver = &Solver1;
btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
convexConvex.m_catchDegeneracies = 1;
// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
btTransform tr;
tr.setIdentity();
input.m_transformA = tr;
input.m_transformB = tr;
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
normal[0] = gjkOutput.m_normalOnBInWorld[0];
normal[1] = gjkOutput.m_normalOnBInWorld[1];
normal[2] = gjkOutput.m_normalOnBInWorld[2];
return gjkOutput.m_distance;
}
return -1.0f;
}
void btSoftBodyHelpers::Draw( btSoftBody* psb,
btIDebugDraw* idraw,
int drawflags)
{
const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5;
const btVector3 lcolor=btVector3(0,0,0);
const btVector3 ncolor=btVector3(1,1,1);
const btVector3 ccolor=btVector3(1,0,0);
int i,j,nj;
/* Clusters */
if(0!=(drawflags&fDrawFlags::Clusters))
{
srand(1806);
for(i=0;i<psb->m_clusters.size();++i)
{
if(psb->m_clusters[i]->m_collide)
{
btVector3 color( rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX);
color=color.normalized()*0.75;
btAlignedObjectArray<btVector3> vertices;
vertices.resize(psb->m_clusters[i]->m_nodes.size());
for(j=0,nj=vertices.size();j<nj;++j)
{
vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
}
#define USE_NEW_CONVEX_HULL_COMPUTER
#ifdef USE_NEW_CONVEX_HULL_COMPUTER
btConvexHullComputer computer;
int stride = sizeof(btVector3);
int count = vertices.size();
btScalar shrink=0.f;
btScalar shrinkClamp=0.f;
computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);
for (int i=0;i<computer.faces.size();i++)
{
int face = computer.faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];
const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();
int v0 = firstEdge->getSourceVertex();
int v1 = firstEdge->getTargetVertex();
while (edge!=firstEdge)
{
int v2 = edge->getTargetVertex();
idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);
edge = edge->getNextEdgeOfFace();
v0=v1;
v1=v2;
};
}
#else
HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
HullResult hres;
HullLibrary hlib;
hdsc.mMaxVertices=vertices.size();
hlib.CreateConvexHull(hdsc,hres);
const btVector3 center=average(hres.m_OutputVertices);
add(hres.m_OutputVertices,-center);
mul(hres.m_OutputVertices,(btScalar)1);
add(hres.m_OutputVertices,center);
for(j=0;j<(int)hres.mNumFaces;++j)
{
const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
hres.m_OutputVertices[idx[1]],
hres.m_OutputVertices[idx[2]],
color,1);
}
hlib.ReleaseResult(hres);
#endif
}
/* Velocities */
#if 0
for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
{
const btSoftBody::Cluster& c=psb->m_clusters[i];
const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
const btVector3 v=c.m_lv+btCross(c.m_av,r);
idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
}
#endif
/* Frame */
// btSoftBody::Cluster& c=*psb->m_clusters[i];
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
}
}
else
{
/* Nodes */
if(0!=(drawflags&fDrawFlags::Nodes))
{
for(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
}
}
/* Links */
if(0!=(drawflags&fDrawFlags::Links))
{
for(i=0;i<psb->m_links.size();++i)
{
const btSoftBody::Link& l=psb->m_links[i];
if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
}
}
/* Normals */
if(0!=(drawflags&fDrawFlags::Normals))
{
for(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 d=n.m_n*nscl;
idraw->drawLine(n.m_x,n.m_x+d,ncolor);
idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
}
}
/* Contacts */
if(0!=(drawflags&fDrawFlags::Contacts))
{
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
for(i=0;i<psb->m_rcontacts.size();++i)
{
const btSoftBody::RContact& c=psb->m_rcontacts[i];
const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*
(btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
const btVector3 y=btCross(x,c.m_cti.m_normal).normalized();
idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
}
}
/* Faces */
if(0!=(drawflags&fDrawFlags::Faces))
{
const btScalar scl=(btScalar)0.8;
const btScalar alp=(btScalar)1;
const btVector3 col(0,(btScalar)0.7,0);
for(i=0;i<psb->m_faces.size();++i)
{
const btSoftBody::Face& f=psb->m_faces[i];
if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
const btVector3 c=(x[0]+x[1]+x[2])/3;
idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
col,alp);
}
}
/* Tetras */
if(0!=(drawflags&fDrawFlags::Tetras))
{
const btScalar scl=(btScalar)0.8;
const btScalar alp=(btScalar)1;
const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);
for(int i=0;i<psb->m_tetras.size();++i)
{
const btSoftBody::Tetra& t=psb->m_tetras[i];
if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
}
}
}
/* Anchors */
if(0!=(drawflags&fDrawFlags::Anchors))
{
for(i=0;i<psb->m_anchors.size();++i)
{
const btSoftBody::Anchor& a=psb->m_anchors[i];
const btVector3 q=a.m_body->getWorldTransform()*a.m_local;
drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
drawVertex(idraw,q,0.25,btVector3(0,1,0));
idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
}
for(i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
if(n.m_im<=0)
{
drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
}
}
}
/* Notes */
if(0!=(drawflags&fDrawFlags::Notes))
{
for(i=0;i<psb->m_notes.size();++i)
{
const btSoftBody::Note& n=psb->m_notes[i];
btVector3 p=n.m_offset;
for(int j=0;j<n.m_rank;++j)
{
p+=n.m_nodes[j]->m_x*n.m_coords[j];
}
idraw->draw3dText(p,n.m_text);
}
}
/* Node tree */
if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);
/* Face tree */
if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);
/* Cluster tree */
if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);
/* Joints */
if(0!=(drawflags&fDrawFlags::Joints))
{
for(i=0;i<psb->m_joints.size();++i)
{
const btSoftBody::Joint* pj=psb->m_joints[i];
switch(pj->Type())
{
case btSoftBody::Joint::eType::Linear:
{
const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
drawVertex(idraw,a0,0.25,btVector3(1,1,0));
drawVertex(idraw,a1,0.25,btVector3(0,1,1));
}
break;
case btSoftBody::Joint::eType::Angular:
{
//const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
break;
}
default:
{
}
}
}
}
}