// 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;
    
}
Пример #4
0
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)),
Пример #6
0
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() );
}
Пример #7
0
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
}
Пример #9
0
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();
    }
}
Пример #10
0
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);
}
Пример #11
0
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;
}
Пример #12
0
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);
				}
			}
		}

	
}
Пример #13
0
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;
}
Пример #14
0
void PhysicsObject::setScale(glm::vec3 vector)
{
	btVector3 scaleAmount = btVector3(vector.x,vector.y,vector.z);
    this->collisionObject->getCollisionShape()->setLocalScaling(scaleAmount);
	this->updateTransformationMatrix();
}
Пример #15
0
btVector3 Vec3f::toBt() const {
    return btVector3(x, y ,z);
}
Пример #16
0
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

	
}
Пример #17
0
//--------------------------------------------------------------
void ofxBulletSoftBody::setNodePositionAt(size_t n, const glm::vec3& pos) {
    _softBody->m_nodes.at(n).m_x = btVector3(pos.x, pos.y, pos.z);
}
Пример #18
0
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();
}
Пример #19
0
plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
{
	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
}
Пример #20
0
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

}
Пример #21
0
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:
				{
				}
					
			}		
		}
	}
}