void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
{
btTransform rayFromTrans,rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
rayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback);
}
}
}
}
btSoftSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btSoftRigidDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback)
: m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld),
m_world(world),
m_resultCallback(resultCallback)
{
m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld);
m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin(m_rayToWorld);
btVector3 rayDir = (rayToWorld - rayFromWorld);
rayDir.normalize();
///what about division by zero? --> just set rayDirection[i] to INF/1e30
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld);
}
void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
if ((*m_data->m_links[linkIndex]).inertial)
{
mass = (*m_data->m_links[linkIndex]).inertial->mass;
localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
} else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
//-----------------------------------------------------------------------
// g e t W o r l d T r a n s f o r m
//-----------------------------------------------------------------------
// getWorldTransform is invoked when any body is initially created. After
// that, it is invoked only for kinematic objects.
void TPhysicsObject::getWorldTransform(btTransform& centerOfMassWorldTrans) const
{
if(!m_sceneNode)
return;
m_sceneNode->updateAbsolutePosition();
/*
TVector3 pos,rot(0,0,0);
pos = m_sceneNode->getAbsolutePosition();
TMatrix4::getRotationDegreesDivScale(m_sceneNode->getAbsoluteTransformation(), rot);
TIBConvert::IrrToBullet(pos, rot, centerOfMassWorldTrans);
*/
centerOfMassWorldTrans.setIdentity();
centerOfMassWorldTrans.setFromOpenGLMatrix(m_sceneNode->getAbsoluteTransformation().pointer());
}
void body(int bodytype, vec3 origin,vec3 size, vec3 inertia,float mass) {
btCollisionShape* aShape = NULL;
pos[0] = origin.x; pos[1] = origin.y; pos[2] = origin.z;
inertia.x = 1;
inertia.y = 1;
inertia.z = 1;
mass = 1;
switch (bodytype) {
case BODYTYPE_BOX:
aShape = new btBoxShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_CYLINDER:
aShape = new btCylinderShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_SPHERE:
aShape = new btSphereShape(btScalar(size.x));
break;
case BODYTYPE_CAPSULE:
aShape = new btCapsuleShape(btScalar(size.x),btScalar(size.y));
break;
case BODYTYPE_CONE:
aShape = new btConeShape(btScalar(size.x),btScalar(size.y));
break;
}
if (aShape) {
btScalar Mass(mass);
bool isDynamic = (Mass != 0.f);
btVector3 localInertia(inertia.x,inertia.y,inertia.z);
if (isDynamic)
aShape->calculateLocalInertia(mass,localInertia);
transform.setIdentity();
transform.setOrigin(btVector3(origin.x,origin.y,origin.z));
btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(Mass,myMotionState,aShape,localInertia);
body1 = new btRigidBody(rbInfo);
collisionShapes.push_back(aShape);
dynamicsWorld->addRigidBody(body1);
}
//return body1;
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
tr.setIdentity();
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
}
}
{
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(rpy_str),logger))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
}
return true;
}
void DynamicsMotionState::getWorldTransform(btTransform& transform) const
{
// DALI_LOG_INFO(Debug::Filter::gDynamics, Debug::Verbose, "%s\n", __PRETTY_FUNCTION__);
// get node's world position and rotation
Vector3 position;
Quaternion rotation;
Vector3 axis;
float angle( 0.0f );
mDynamicsBody.GetNodePositionAndRotation(position, rotation);
rotation.ToAxisAngle( axis, angle );
// modify parameters
transform.setIdentity();
transform.setOrigin(btVector3(position.x, position.y, position.z));
if( axis != Vector3::ZERO )
{
transform.setRotation( btQuaternion(btVector3(axis.x, axis.y, axis.z), btScalar(angle)) );
}
}
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
mass = link->m_inertia.m_mass;
inertialFrame = link->m_inertia.m_linkLocalFrame;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
{
GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(32);
float extraScaling = 1;//0.01;
btHashMap<btHashString, int> name2ShapeIndex;
b3FileUtils f;
char filename[1024];
if (!f.findFile(relativeFileName,filename,1024))
{
printf("File not found: %s\n", filename);
return;
}
TiXmlDocument doc(filename);
if (!doc.LoadFile())
return;
//We need units to be in meter, so apply a scaling using the asset/units meter
unitMeterScaling=1;
upAxisTransform.setIdentity();
//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis);
btMatrix4x4 ident;
ident.setIdentity();
readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
}
bool world_transform(std::string frame_id,
const planning_models::KinematicState &state,
btTransform &transform) {
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId())) {
//identity transform
transform.setIdentity();
return true;
}
if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId())) {
transform = state.getRootTransform();
return true;
}
const planning_models::KinematicState::LinkState *link =
state.getLinkState(frame_id);
if (!link) {
ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str());
return false;
}
transform = link->getGlobalLinkTransform();
return true;
}
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
jointDamping = 0.f;
jointFriction = 0.f;
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
if (link->m_parentJoint)
{
UrdfJoint* pj = link->m_parentJoint;
parent2joint = pj->m_parentLinkToJointTransform;
jointType = pj->m_type;
jointAxisInJointSpace = pj->m_localJointAxis;
jointLowerLimit = pj->m_lowerLimit;
jointUpperLimit = pj->m_upperLimit;
jointDamping = pj->m_jointDamping;
jointFriction = pj->m_jointFriction;
return true;
} else
{
parent2joint.setIdentity();
return false;
}
}
return false;
}
void btTransformFromIrrlichtMatrix(const irr::core::matrix4& irrmat, btTransform &transform)
{
transform.setIdentity();
transform.setFromOpenGLMatrix(irrmat.pointer());
}
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
btMatrix3x3 linkInertiaBasis;
btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
linkMass = 0.f;
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
else
{
linkMass = link->m_inertia.m_mass;
if (link->m_inertia.m_ixy == 0.0 &&
link->m_inertia.m_ixz == 0.0 &&
link->m_inertia.m_iyz == 0.0)
{
principalInertiaX = link->m_inertia.m_ixx;
principalInertiaY = link->m_inertia.m_iyy;
principalInertiaZ = link->m_inertia.m_izz;
linkInertiaBasis.setIdentity();
}
else
{
principalInertiaX = link->m_inertia.m_ixx;
btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btScalar threshold = 1.0e-6;
int numIterations = 30;
inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
principalInertiaX = inertiaTensor[0][0];
principalInertiaY = inertiaTensor[1][1];
principalInertiaZ = inertiaTensor[2][2];
}
}
mass = linkMass;
if (principalInertiaX < 0 ||
principalInertiaX > (principalInertiaY + principalInertiaZ) ||
principalInertiaY < 0 ||
principalInertiaY > (principalInertiaX + principalInertiaZ) ||
principalInertiaZ < 0 ||
principalInertiaZ > (principalInertiaX + principalInertiaY))
{
b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
void CcdPhysicsDemo::initPhysics()
{
setTexturing(true);
setShadows(false);
#ifdef USE_PARALLEL_DISPATCHER
#ifdef _WIN32
m_threadSupportSolver = 0;
m_threadSupportCollision = 0;
#endif //
#endif
//#define USE_GROUND_PLANE 1
#ifdef USE_GROUND_PLANE
m_collisionShapes.push_back(new btStaticPlaneShape(btVector3(0,1,0),0.5));
#else
///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate.
///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346
m_collisionShapes.push_back(new btBoxShape (btVector3(200,CUBE_HALF_EXTENTS,200)));
#endif
#ifdef DO_BENCHMARK_PYRAMIDS
m_collisionShapes.push_back(new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)));
#else
// m_collisionShapes.push_back(new btBoxShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)));
m_collisionShapes.push_back(new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)));
#endif
#ifdef DO_BENCHMARK_PYRAMIDS
setCameraDistance(32.5f);
#endif
#ifdef DO_BENCHMARK_PYRAMIDS
m_azi = 90.f;
#endif //DO_BENCHMARK_PYRAMIDS
m_dispatcher=0;
m_collisionConfiguration = new btDefaultCollisionConfiguration();
#ifdef USE_PARALLEL_DISPATCHER
int maxNumOutstandingTasks = 4;
#ifdef USE_WIN32_THREADING
m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
"collision",
processCollisionTask,
createCollisionLocalStoreMemory,
maxNumOutstandingTasks));
#else
#ifdef USE_LIBSPE2
spe_program_handle_t * program_handle;
#ifndef USE_CESOF
program_handle = spe_image_open ("./spuCollision.elf");
if (program_handle == NULL)
{
perror( "SPU OPEN IMAGE ERROR\n");
}
else
{
printf( "IMAGE OPENED\n");
}
#else
extern spe_program_handle_t spu_program;
program_handle = &spu_program;
#endif
SpuLibspe2Support* threadSupportCollision = new SpuLibspe2Support( program_handle, maxNumOutstandingTasks);
#endif //USE_LIBSPE2
///Playstation 3 SPU (SPURS) version is available through PS3 Devnet
/// For Unix/Mac someone could implement a pthreads version of btThreadSupportInterface?
///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
#endif
m_dispatcher = new SpuGatheringCollisionDispatcher(m_threadSupportCollision,maxNumOutstandingTasks,m_collisionConfiguration);
// m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
#else
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
#endif //USE_PARALLEL_DISPATCHER
#ifdef USE_CUSTOM_NEAR_CALLBACK
//this is optional
m_dispatcher->setNearCallback(customNearCallback);
#endif
m_broadphase = new btDbvtBroadphase();
#ifdef COMPARE_WITH_QUICKSTEP
m_solver = new btOdeQuickstepConstraintSolver();
#else
#ifdef USE_PARALLEL_SOLVER
m_threadSupportSolver = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
"solver",
processSolverTask,
createSolverLocalStoreMemory,
maxNumOutstandingTasks));
m_solver = new btParallelSequentialImpulseSolver(m_threadSupportSolver,maxNumOutstandingTasks);
#else
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
m_solver = solver;//new btOdeQuickstepConstraintSolver();
#endif //USE_PARALLEL_SOLVER
#endif
btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld = world;
///SOLVER_RANDMIZE_ORDER makes cylinder stacking a bit more stable
world->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
#ifdef USER_DEFINED_FRICTION_MODEL
//user defined friction model is not supported in 'cache friendly' solver yet, so switch to old solver
world->getSolverInfo().m_solverMode = SOLVER_RANDMIZE_ORDER;
#endif //USER_DEFINED_FRICTION_MODEL
#ifdef DO_BENCHMARK_PYRAMIDS
world->getSolverInfo().m_numIterations = 4;
#endif //DO_BENCHMARK_PYRAMIDS
m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
#ifdef USER_DEFINED_FRICTION_MODEL
{
//m_solver->setContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1);
solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,USER_CONTACT_SOLVER_TYPE1);
//m_physicsEnvironmentPtr->setNumIterations(2);
}
#endif //USER_DEFINED_FRICTION_MODEL
int i;
btTransform tr;
tr.setIdentity();
for (i=0;i<gNumObjects;i++)
{
if (i>0)
{
shapeIndex[i] = 1;//sphere
}
else
shapeIndex[i] = 0;
}
if (useCompound)
{
btCompoundShape* compoundShape = new btCompoundShape();
btCollisionShape* oldShape = m_collisionShapes[1];
m_collisionShapes[1] = compoundShape;
btVector3 sphereOffset(0,0,2);
comOffset.setIdentity();
#ifdef CENTER_OF_MASS_SHIFT
comOffset.setOrigin(comOffsetVec);
compoundShape->addChildShape(comOffset,oldShape);
#else
compoundShape->addChildShape(tr,oldShape);
tr.setOrigin(sphereOffset);
compoundShape->addChildShape(tr,new btSphereShape(0.9));
#endif
}
#ifdef DO_WALL
for (i=0;i<gNumObjects;i++)
{
btCollisionShape* shape = m_collisionShapes[shapeIndex[i]];
shape->setMargin(gCollisionMargin);
bool isDyna = i>0;
btTransform trans;
trans.setIdentity();
if (i>0)
{
//stack them
int colsize = 10;
int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS);
int row2 = row;
int col = (i)%(colsize)-colsize/2;
if (col>3)
{
col=11;
row2 |=1;
}
btVector3 pos(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS,
row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0);
trans.setOrigin(pos);
} else
{
trans.setOrigin(btVector3(0,EXTRA_HEIGHT-CUBE_HALF_EXTENTS,0));
}
float mass = 1.f;
if (!isDyna)
mass = 0.f;
btRigidBody* body = localCreateRigidBody(mass,trans,shape);
#ifdef USE_KINEMATIC_GROUND
if (mass == 0.f)
{
body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
body->setActivationState(DISABLE_DEACTIVATION);
}
#endif //USE_KINEMATIC_GROUND
// Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS
body->setCcdMotionThreshold( CUBE_HALF_EXTENTS );
//Experimental: better estimation of CCD Time of Impact:
body->setCcdSweptSphereRadius( 0.2*CUBE_HALF_EXTENTS );
#ifdef USER_DEFINED_FRICTION_MODEL
///Advanced use: override the friction solver
body->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1;
#endif //USER_DEFINED_FRICTION_MODEL
}
#endif
#ifdef DO_BENCHMARK_PYRAMIDS
btTransform trans;
trans.setIdentity();
btScalar halfExtents = CUBE_HALF_EXTENTS;
trans.setOrigin(btVector3(0,-halfExtents,0));
localCreateRigidBody(0.f,trans,m_collisionShapes[shapeIndex[0]]);
int numWalls = 15;
int wallHeight = 15;
float wallDistance = 3;
for (int i=0;i<numWalls;i++)
{
float zPos = (i-numWalls/2) * wallDistance;
createStack(m_collisionShapes[shapeIndex[1]],halfExtents,wallHeight,zPos);
}
// createStack(m_collisionShapes[shapeIndex[1]],halfExtends,20,10);
// createStack(m_collisionShapes[shapeIndex[1]],halfExtends,20,20);
#define DESTROYER_BALL 1
#ifdef DESTROYER_BALL
btTransform sphereTrans;
sphereTrans.setIdentity();
sphereTrans.setOrigin(btVector3(0,2,40));
btSphereShape* ball = new btSphereShape(2.f);
m_collisionShapes.push_back(ball);
btRigidBody* ballBody = localCreateRigidBody(10000.f,sphereTrans,ball);
ballBody->setLinearVelocity(btVector3(0,0,-10));
#endif
#endif //DO_BENCHMARK_PYRAMIDS
// clientResetScene();
}
EGLRendererObjectArray()
{
m_worldTransform.setIdentity();
m_localScaling.setValue(1, 1, 1);
m_graphicsInstanceId = -1;
}
void BulletTestApp::initTest()
{
// Clean up last test
if ( mWorld ) {
mWorld->clear();
}
// Used when generating terrain
Channel32f heightField;
// Create and add the wobbly box
mGroundTransform.setIdentity();
switch ( mTest ) {
case 0:
mGround = bullet::createRigidBox( mWorld, Vec3f( 200.0f, 35.0f, 200.0f ), 0.0f );
bullet::createRigidSphere( mWorld, 50.0f, 64, 0.0f, Vec3f( 0.0f, -50.0f, 0.0f ) );
break;
case 1:
mGround = bullet::createRigidHull( mWorld, mConvex, Vec3f::one() * 50.0f, 0.0f );
break;
case 2:
mGround = bullet::createRigidMesh( mWorld, mConcave, Vec3f( 10.0f, 1.0f, 10.0f ), 0.0f, 0.0f );
break;
case 3:
mGround = bullet::createRigidBox( mWorld, Vec3f( 200.0f, 35.0f, 200.0f ), 0.0f );
break;
case 4:
mGround = bullet::createRigidBox( mWorld, Vec3f( 200.0f, 35.0f, 200.0f ), 0.0f );
break;
case 5:
heightField = Channel32f( loadImage( loadResource( RES_IMAGE_HEIGHTFIELD_SM ) ) );
mGround = bullet::createRigidTerrain( mWorld, heightField, -1.0f, 1.0f, Vec3f( 6.0f, 80.0f, 6.0f ), 0.0f );
break;
case 6:
// ADVANCED: To add a custom class, create a standard pointer to it and
// pushBack it into your world. Be sure to delete this pointer when you no loinger need it
heightField = Channel32f( loadImage( loadResource( RES_IMAGE_HEIGHTFIELD ) ) );
mTerrain = new DynamicTerrain( heightField, -1.0f, 1.0f, Vec3f( 2.0f, 50.0f, 2.0f ), 0.0f );
mWorld->pushBack( mTerrain );
break;
case 7:
// Start capture
if ( !mCapture ) {
mCapture = Capture( 320, 240 );
mCapture.start();
}
break;
}
// Set friction for box
if ( mTest < 5 ) {
btRigidBody* boxBody = bullet::toBulletRigidBody( mGround );
boxBody->setFriction( 0.95f );
}
}
BasicBlendReader(btDynamicsWorld* dynamicsWorld, BasicDemo* basicDemo)
:BulletBlendReader(dynamicsWorld),
m_basicDemo(basicDemo)
{
m_cameraTrans.setIdentity();
}
void BulletTestApp::initTest()
{
// Clean up last test
if ( mWorld ) {
mWorld->clear();
mWorld->getInfo().m_sparsesdf.Reset();
}
// Used when generating terrain
Channel32f heightField;
// Create and add the wobbly box
btSoftBody* body = 0;
btSoftBody::Material* material = 0;
mGroundTransform.setIdentity();
switch ( mTest ) {
case 0:
mGround = bullet::createRigidBox( mWorld, Vec3f( 200.0f, 35.0f, 200.0f ), 0.0f );
bullet::createRigidSphere( mWorld, 50.0f, 0.0f, Vec3f( 0.0f, -50.0f, 0.0f ) );
break;
case 1:
mGround = bullet::createRigidHull( mWorld, mConvex, Vec3f::one() * 50.0f, 0.0f );
break;
case 2:
mGround = bullet::createRigidMesh( mWorld, mConcave, Vec3f( 10.0f, 1.0f, 10.0f ), 0.0f, 0.0f );
break;
case 5:
heightField = Channel32f( loadImage( loadResource( RES_IMAGE_HEIGHTFIELD_SM ) ) );
mGround = bullet::createRigidTerrain( mWorld, heightField, -1.0f, 1.0f, Vec3f( 6.0f, 80.0f, 6.0f ), 0.0f );
break;
case 6:
// ADVANCED: To add a custom class, create a standard pointer to it and
// pushBack it into your world. Be sure to delete this pointer when you no longer need it.
heightField = Channel32f( loadImage( loadResource( RES_IMAGE_HEIGHTFIELD ) ) );
mTerrain = new DynamicTerrain( heightField, -1.0f, 1.0f, Vec3f( 2.0f, 50.0f, 2.0f ), 0.0f );
mWorld->pushBack( mTerrain );
break;
case 7:
// Start capture
if ( !mCapture ) {
mCapture = Capture( 320, 240 );
mCapture.start();
}
break;
case 8:
mWorld->getInfo().m_gravity = toBulletVector3( Vec3f( 0.0f, -0.5f, 0.0f ) );
mGround = bullet::createSoftCloth( mWorld, Vec2f::one() * 180.0f, Vec2i( 18, 20 ), SoftCloth::CLOTH_ATTACH_CORNER_ALL, Vec3f( 0.0f, 0.0f, 40.0f ), Quatf( 0.0f, 0.0f, 0.0f, 0.1f ) );
body = toBulletSoftBody( mGround );
body->m_materials[ 0 ]->m_kAST = 0.2f;
body->m_materials[ 0 ]->m_kLST = 0.2f;
body->m_materials[ 0 ]->m_kVST = 0.2f;
body->m_cfg.kDF = 0.9f;
body->m_cfg.kSRHR_CL = 1.0f;
body->m_cfg.kSR_SPLT_CL = 0.0f;
body->m_cfg.collisions = btSoftBody::fCollision::CL_SS + btSoftBody::fCollision::CL_RS;
body->getCollisionShape()->setMargin( 0.0001f );
body->setTotalMass( 500.0f );
body->generateClusters( 0 );
material = body->appendMaterial();
material->m_kAST = 0.5f;
material->m_kLST = 0.5f;
material->m_kVST = 1.0f;
body->generateBendingConstraints( 1, material );
break;
case 9:
heightField = Channel32f( loadImage( loadResource( RES_IMAGE_HEIGHTFIELD_SM ) ) );
mGround = bullet::createRigidTerrain( mWorld, heightField, -1.0f, 1.0f, Vec3f( 6.0f, 80.0f, 6.0f ), 0.0f );
break;
default:
mGround = bullet::createRigidBox( mWorld, Vec3f( 200.0f, 35.0f, 200.0f ), 0.0f );
break;
}
// Set friction for box
if ( mTest < 5 ) {
btRigidBody* boxBody = bullet::toBulletRigidBody( mGround );
boxBody->setFriction( 0.95f );
}
}
int main(int argc, char** argv)
{
//*** init Bullet Physics
btQuaternion qtn;
btCollisionShape *shape;
btDefaultMotionState *motionState;
btDefaultCollisionConfiguration *collisionCfg
= new btDefaultCollisionConfiguration();
btAxisSweep3 *axisSweep
= new btAxisSweep3(btVector3(-100,-100,-100), btVector3(100,100,100), 128);
dynamicsWorld = new btDiscreteDynamicsWorld(new btCollisionDispatcher(collisionCfg),
axisSweep, new btSequentialImpulseConstraintSolver, collisionCfg);
dynamicsWorld->setGravity(btVector3(0, -10, 0));
//*** box1 - STATIC / mass=btScalar(0.0)
shape = new btBoxShape(btVector3(20,20,20));
trans.setIdentity();
qtn.setEuler(0, 0.0, 0.0);
trans.setRotation(qtn);
trans.setOrigin(btVector3(0, -20, 0));
motionState = new btDefaultMotionState(trans);
btRigidBody::btRigidBodyConstructionInfo boxRigidBodyCI(btScalar(0.0), motionState, shape, btVector3(0,0,0));
boxRigidBodyCI.m_friction = 0.4;
box1 = new btRigidBody(boxRigidBodyCI);
box1->setCollisionFlags( box1->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
box1->setActivationState(DISABLE_DEACTIVATION);
dynamicsWorld->addRigidBody(box1);
//*** box3 - STATIC / mass=btScalar(0.0)
shape = new btBoxShape(btVector3(15,15,15));
trans.setIdentity();
qtn.setEuler(0, 0.0, 0.0);
trans.setRotation(qtn);
trans.setOrigin(btVector3(-35, -35, 0));
motionState = new btDefaultMotionState(trans);
btRigidBody::btRigidBodyConstructionInfo boxRigidBodyCI3(btScalar(0.0), motionState, shape, btVector3(0,0,0));
boxRigidBodyCI3.m_friction = 0.4;
box3 = new btRigidBody(boxRigidBodyCI3);
dynamicsWorld->addRigidBody(box3);
//*** box2 - DYNAMIC / mass=btScalar(1.0)
shape = new btBoxShape(btVector3(5,5,5));
trans.setIdentity();
qtn.setEuler(0.8, 0.7, 0.4);
trans.setRotation(qtn);
trans.setOrigin(btVector3(-10, 50, 0));
motionState = new btDefaultMotionState(trans);
btScalar mass = 1;
btVector3 Intertia(0,0,0);
shape->calculateLocalInertia(mass, Intertia);
btRigidBody::btRigidBodyConstructionInfo *boxRigidBodyCI2;
boxRigidBodyCI2 = new btRigidBody::btRigidBodyConstructionInfo(mass, motionState, shape, Intertia);
boxRigidBodyCI2->m_restitution = 0.4;
box2 = new btRigidBody(*boxRigidBodyCI2);
box2->setActivationState(DISABLE_DEACTIVATION);
//box2->setLinearFactor(btVector3(1,1,0));
//box2->setAngularFactor(btVector3(0,0,1));
dynamicsWorld->addRigidBody(box2);
//*** init GLUT
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutCreateWindow("Jump you little bastard");
glutDisplayFunc(draw);
glutIdleFunc(tim);
glutKeyboardFunc(keyPress);
//*** init OpenGL
glEnable(GL_CULL_FACE);
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
glEnable(GL_COLOR_MATERIAL);
glMatrixMode(GL_PROJECTION);
gluPerspective( 90.0, 1.0, 1.0, 1000.0);
glMatrixMode(GL_MODELVIEW);
gluLookAt(0.0, 5.0, 60.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
glutMainLoop();
//*** EXIT
delete shape;
delete motionState;
delete collisionCfg;
delete axisSweep;
}
// *****************************************************************************
// *****************************************************************************
// *****************************************************************************
int main(int argc, char **argv)
{
// *************************************************************************
// If no command line arguments were passed, check if there is an args file
// at ~/.spinFramework/args and override argc and argv with those:
std::vector<char*> newArgs = spin::getUserArgs();
if ((argc==1) && (newArgs.size() > 1))
{
// need first arg (command name):
newArgs.insert(newArgs.begin(), argv[0]);
argc = (int)newArgs.size()-1;
argv = &newArgs[0];
}
using namespace spin;
spinClientContext spinListener;
spinApp &spin = spinApp::Instance();
std::string userID;
double maxFrameRate = 60;
std::string sceneID = spin.getSceneID();
// *************************************************************************
// get arguments:
osg::ArgumentParser arguments(&argc,argv);
// set up the usage document, which a user can acess with -h or --help
arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
arguments.getApplicationUsage()->setDescription(arguments.getApplicationName()+" is a 3D viewer for the SPIN Framework.");
arguments.getApplicationUsage()->setCommandLineUsage(arguments.getApplicationName()+" [options]");
// add generic spin arguments (for address/port setup, scene-id, etc)
spinListener.addCommandLineOptions(&arguments);
// arguments specific to spinviewer:
arguments.getApplicationUsage()->addCommandLineOption("--user-id <uniqueID>", "Specify a user ID for this viewer (Default: <this computer's name>)");
// *************************************************************************
// PARSE ARGS:
if (!spinListener.parseCommandLineOptions(&arguments))
return 0;
osg::ArgumentParser::Parameter param_userID(userID);
arguments.read("--user-id", param_userID);
if (not userID.empty())
spin.setUserID(userID);
// *************************************************************************
// construct the viewer:
// (note, this constructor gets rid of some additional args)
osgViewer::CompositeViewer viewer = osgViewer::CompositeViewer(arguments);
viewer.setThreadingModel(osgViewer::CompositeViewer::CullDrawThreadPerContext);
viewer.getUsage(*arguments.getApplicationUsage());
// *************************************************************************
// start the listener thread:
if (!spinListener.start())
{
std::cout << "ERROR: could not start SPIN listener" << std::endl;
exit(EXIT_FAILURE);
}
spin.sceneManager->setGraphical(true);
// *************************************************************************
// get details on keyboard and mouse bindings used by the viewer.
viewer.getUsage(*arguments.getApplicationUsage());
// *************************************************************************
// set up initial view:
osg::ref_ptr<osgViewer::View> view = new osgViewer::View;
view->setSceneData(spin.sceneManager->rootNode.get());
view->setUpViewInWindow(50,50,800,600,0);
viewer.addView(view.get());
view->getCamera()->setClearColor(osg::Vec4(0.0, 0.0, 0.0, 0.0));
view->addEventHandler(new osgViewer::StatsHandler);
view->addEventHandler(new osgViewer::ThreadingHandler);
view->addEventHandler(new osgViewer::WindowSizeHandler);
view->addEventHandler(new osgViewer::HelpHandler(arguments.getApplicationUsage()));
view->addEventHandler( new osgGA::StateSetManipulator(view->getCamera()->getOrCreateStateSet()) );
//view->setLightingMode(osg::View::NO_LIGHT);
view->setLightingMode(osg::View::HEADLIGHT);
//view->setLightingMode(osg::View::SKY_LIGHT);
osg::ref_ptr<ViewerManipulator> manipulator = new ViewerManipulator();
view->setCameraManipulator(manipulator.get());
// *************************************************************************
// register an extra OSC callback so that we can spy on OSC messages:
/*
std::vector<lo_server>::iterator servIter;
for (servIter = spin.getContext()->lo_rxServs_.begin(); servIter != spin.getContext()->lo_rxServs_.end(); ++servIter)
{
lo_server_add_method((*servIter), NULL, NULL, message_callback, NULL);
}
*/
// *************************************************************************
// For testing purposes, we allow loading a scene with a commandline arg:
//osg::setNotifyLevel(osg::INFO);
btVector3 worldAabbMin(-1000, -1000, -1000);
btVector3 worldAabbMax(1000, 1000, 1000);
const int maxProxies = 32766;
// register global callback
gContactAddedCallback = materialCombinerCallback;
btSequentialImpulseConstraintSolver *solver = new btSequentialImpulseConstraintSolver;
btAxisSweep3 *broadphase = new btAxisSweep3(worldAabbMin, worldAabbMax, maxProxies);
btDefaultCollisionConfiguration *collisionConfiguration = new btDefaultCollisionConfiguration();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
btDynamicsWorld *m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
//m_dynamicsWorld->setGravity(btVector3(0, 0, -9.81));
m_dynamicsWorld->setGravity(btVector3(0, 0, -10.0));
// ShapeNode
osg::ref_ptr<ShapeNode> shp = dynamic_cast<spin::ShapeNode*>(spin.sceneManager->createNode("shp", "ShapeNode"));
shp->setTranslation(-2.0, 0.0, 5.0);
shp->setOrientation(0.0, 45.0, 0.0);
shp->setInteractionMode(spin::GroupNode::DRAG);
std::cout << "created shp with osg name: " << shp->getName() << std::endl;
if (1)
{
btRigidBody* shp_body;
btCollisionShape *shp_collisionObj;
shp->setTranslation(-2.0, 0.0, 2.0);
shp->setOrientation(0.0, 45.0, 0.0);
shp->setInteractionMode(spin::GroupNode::DRAG);
shp_collisionObj = new btBoxShape(btVector3(1, 1, 1));
// TODO: use global position: shp->getGlobalMatrix
btVector3 pos(shp->getTranslation().x(), shp->getTranslation().y(), shp->getTranslation().z());
btQuaternion quat(shp->getOrientationQuat().x(), shp->getOrientationQuat().y(), shp->getOrientationQuat().z(), shp->getOrientationQuat().w());
btTransform trans;
trans.setIdentity();
trans.setOrigin(pos);
//trans.setRotation(quat);
btScalar mass = 1.f;
shp_body = createRigidBody(m_dynamicsWorld, mass, trans, shp_collisionObj);
shp_body->setUserPointer(shp.get());
shp_body->setCollisionFlags(shp_body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
//shp_body->setCcdMotionThreshold(0.5);
//shp_body->setCcdSweptSphereRadius(0.2 * 0.5);
shp->setUpdateCallback(new BallUpdateCallback(shp_body));
// register an extra OSC callback so that we can spy on OSC messages:
std::vector<lo_server>::iterator servIter;
for (servIter = spin.getContext()->lo_rxServs_.begin(); servIter != spin.getContext()->lo_rxServs_.end(); ++servIter)
{
lo_server_add_method((*servIter), std::string("/SPIN/"+spin.getSceneID()+"/shp").c_str(), NULL, shpCallback, &shp_body);
}
}
// ball
osg::ref_ptr<osg::Node> ball;
osg::ref_ptr<osg::PositionAttitudeTransform> ball_pat;
if (0) {
btRigidBody* lnd;
btCollisionShape *lnd_shape;
ball = osgDB::readNodeFile("ball.osg");
ball->setName("ball");
ball_pat = new osg::PositionAttitudeTransform;
ball_pat->setScale(osg::Vec3(5, 1, 1));
ball_pat->setAttitude(osg::Quat(osg::DegreesToRadians(45.0),osg::Y_AXIS));
ball_pat->addChild(ball.get());
spin.sceneManager->rootNode->addChild(ball_pat.get());
lnd_shape = new btBoxShape(btVector3(ball_pat->getScale().x(), ball_pat->getScale().y(), ball_pat->getScale().z()));
//lnd_shape = new btBoxShape(btVector3(1, 1, 1));
btVector3 pos(0, 0, 20.0);
btTransform trans;
trans.setIdentity();
trans.setOrigin(pos);
btQuaternion quat(ball_pat->getAttitude().x(), ball_pat->getAttitude().y(), ball_pat->getAttitude().z(), ball_pat->getAttitude().w());
trans.setRotation(quat);
btScalar mass = 1.f;
lnd = createRigidBody(m_dynamicsWorld, mass, trans, lnd_shape);
lnd->setUserPointer(ball.get());
lnd->setCollisionFlags(lnd->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
lnd->setCcdMotionThreshold(0.5);
lnd->setCcdSweptSphereRadius(0.2 * 0.5);
ball_pat->setUpdateCallback(new BallUpdateCallback(lnd));
}
// ground plane
osg::ref_ptr<osg::Node> groundPlane;
osg::ref_ptr<osg::PositionAttitudeTransform> ground_pat;
{
groundPlane = osgDB::readNodeFile("ground-plane.osg");
//groundPlane->setName("groundPlane");
ground_pat = new osg::PositionAttitudeTransform;
ground_pat->addChild(groundPlane.get());
ground_pat->setScale(osg::Vec3(100.0, 100.0, 0.1));
spin.sceneManager->rootNode->addChild(ground_pat.get());
btCollisionShape *gnd_shape = new btStaticPlaneShape(btVector3(0, 0, 1), 0.5);
btVector3 pos(0, 0, -0.5);
btTransform trans;
trans.setIdentity();
trans.setOrigin(pos);
btScalar mass = 0.f; // 0 makes this a static (unmovable) object
btRigidBody* gnd = createRigidBody(m_dynamicsWorld, mass, trans, gnd_shape);
gnd->setUserPointer(groundPlane.get());
gnd->setCollisionFlags(gnd->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT
| btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
gnd->setActivationState(DISABLE_DEACTIVATION);
}
// move view back:
spin.userNode->setTranslation(0,-50,3);
spin.sceneManager->createNode("grid", "GridNode");
// *************************************************************************
// any option left unread are converted into errors to write out later.
arguments.reportRemainingOptionsAsUnrecognized();
// report any errors if they have occured when parsing the program aguments.
if (arguments.errors())
{
arguments.writeErrorMessages(std::cout);
return 1;
}
// *************************************************************************
// start threads:
viewer.realize();
// Try to subscribe with the current (default or manually specified) TCP
// subscription information. If this fails, it's likely because the server
// is not online, but when it comes online, it will send a userRefresh
// message which will invoke another subscription attempt:
spinListener.subscribe();
// ask for refresh:
spin.SceneMessage("s", "refresh", LO_ARGS_END);
osg::Timer_t lastFrameTick = osg::Timer::instance()->tick();
double minFrameTime = 1.0 / maxFrameRate;
std::cout << "\nspinviewer is READY" << std::endl;
// program loop:
while(not viewer.done())
{
if (spinListener.isRunning())
{
double dt = osg::Timer::instance()->delta_s(lastFrameTick, osg::Timer::instance()->tick());
if (dt >= minFrameTime)
{
viewer.advance();
viewer.eventTraversal();
pthread_mutex_lock(&sceneMutex);
spin.sceneManager->update();
//physics update:
m_dynamicsWorld->stepSimulation(dt); //, 10, 0.01);
m_dynamicsWorld->updateAabbs();
viewer.updateTraversal();
viewer.renderingTraversals();
pthread_mutex_unlock(&sceneMutex);
// save time when the last time a frame was rendered:
lastFrameTick = osg::Timer::instance()->tick();
dt = 0;
}
unsigned int sleepTime;
if (!recv) sleepTime = static_cast<unsigned int>(1000000.0*(minFrameTime-dt));
else sleepTime = 0;
if (sleepTime > 100) sleepTime = 100;
if (!recv) OpenThreads::Thread::microSleep(sleepTime);
} else {
for (int i=0; i<viewer.getNumViews(); i++)
{
// this should automatically release the manipulator (right??)
viewer.getView(i)->setCameraManipulator(NULL);
}
// just in case, we'll check if the manipulator is still around:
if (manipulator.valid())
{
manipulator.release();
}
viewer.setDone(true);
}
}
// make sure we're done in case we didn't quit via interrupt
spinListener.stop();
return 0;
}
void BulletTestApp::initTest()
{
// Clean up last test
if ( mWorld )
{
mWorld->clear();
}
mGroundTransform.setIdentity();
// Create the ground
{
mGround = bullet::createRigidBox( mWorld, Vec3f( 357.0f, 1.0f, 600.0f ), 0.0f );
btRigidBody* boxBody = bullet::toBulletRigidBody( mGround );
boxBody->setFriction( 0.95f );
boxBody->setRestitution( .65f );
}
// Create the backboard
{
mBackBaord = bullet::createRigidBox( mWorld, Vec3f( 110.0f, 70.0f, 1.0f ), 0.0f, Vec3f( 0.0f, 100.f, 250.f ));
btRigidBody* boxBody = bullet::toBulletRigidBody( mBackBaord );
boxBody->setFriction( 0.95f );
boxBody->setRestitution( .65f );
}
// {
// mTest = bullet::createRigidCylinder( mWorld, Vec3f( 10.0f, 50.0f, 10.0f ), 16, 0.0f, Vec3f( 0.0f, 50.0f, 0.0f ));
// btRigidBody* boxBody = bullet::toBulletRigidBody( mBackBaord );
// boxBody->setFriction( 0.95f );
// boxBody->setRestitution( .65f );
// }
// Create the ring box
{
mBox = bullet::createRigidBox( mWorld, Vec3f( 5.f, 1.0f, 5.0f ), 0.0f, Vec3f( 0.0f, 75.0f, 247.5f ));
btRigidBody* boxBody = bullet::toBulletRigidBody( mBox );
boxBody->setFriction( 0.95f );
boxBody->setRestitution( .65f );
}
// Create the ring
{
mRing = bullet::createRigidTorus( mWorld, 0.5f, 10.f, 200, 0.0f, Vec3f( 0.0f, 75.0f, 234.0f ), Quatf( Vec3f::xAxis(), M_PI / 2. ) );
btRigidBody* boxBody = bullet::toBulletRigidBody( mRing );
boxBody->setFriction( 0.95f );
boxBody->setRestitution( .65f );
// mRing = bullet::createRigidMesh( mWorld, mTorus, Vec3f( 1.0f, 2.0f, 1.0f ), 0.0f, 0.0f, Vec3f( 0.0f, 50.0f, 0.0f ));
// btRigidBody* boxBody = bullet::toBulletRigidBody( mRing );
// boxBody->setFriction( 0.95f );
// boxBody->setRestitution( .65f );
// float radiusIn = 0.5f;
// float radiusOut = 8.0f;
// float subdivisions = 36.f;
// Vec3f forward( 0.0 , 0.0, 1.0 );
// Vec3f side ( radiusOut, 0.0, 0.0 );
//
// float gap = sqrt( 2.0f * radiusOut * radiusOut - 2.0f * radiusIn * radiusIn * cos(( 2.0f * SIMD_PI ) / subdivisions ));
//
// btTransform t;
// for( int x = 0; x < (int)subdivisions; x++ )
// {
// float angle = ( x * 2.0f * SIMD_PI ) / subdivisions;
// Vec3f position = side;
// position.rotate( forward, angle );
// Quatf q( forward, angle );
//
// mRing = bullet::createRigidCylinder( mWorld, Vec3f( radiusIn, 3* ( SIMD_PI / subdivisions ) + 0.5f * gap, radiusIn ), subdivisions, 0.0f, position, q );
// btRigidBody* boxBody = bullet::toBulletRigidBody( mRing );
// boxBody->setFriction( 0.95f );
// boxBody->setRestitution( .65f );
// }
// mRing = bullet::createRigidCylinder( mWorld, Vec3f( 1, 1, 1 ), 16, 0.0f, Vec3f( 0, 10, 0 ), Quatf( Vec3f::xAxis(), M_PI / 2 ));
}
}
int main ( int argc, char **argv )
{
// On cr�e notre fen�tre gr�ce � SFML
Application.Create( sf::VideoMode( 800, 500, 32 ), "SFML : Bullet physics", sf::Style::Titlebar | sf::Style::Resize | sf::Style::Close);
//Application.Create(sf::VideoMode::GetMode(0), "SFML Window", sf::Style::Fullscreen);
// Creation d'une fen�tre plein �cran avec le meilleur mode vid�o
/// Bullet physics
///collision configuration contains default setup for memory, collision setup
myCollisionConfiguration = new btDefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
myDispatcher = new btCollisionDispatcher(myCollisionConfiguration);
myBroadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
mySequentialImpulseConstraintSolver = new btSequentialImpulseConstraintSolver;
// initialisation du monde bullet
myWorld = new btDiscreteDynamicsWorld(myDispatcher,myBroadphase,mySequentialImpulseConstraintSolver,myCollisionConfiguration);
// On d�finit la gravit�, de fa�on � ce que les objets tombent vers le bas (-Y).
myWorld->setGravity( btVector3(0,-10,0) );
/// SOL ///////////////////////////////////////////
// create a shape
btCollisionShape* shape_sol = new btBoxShape( btVector3(100,1,100) );
myTransform.setIdentity();
myTransform.setOrigin( btVector3(0,0,0) );
btVector3 localInertiaSol(0,0,0);
btScalar mass = 0;
// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
myMotionState_Sol = new btDefaultMotionState(myTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo_sol( mass, myMotionState_Sol, shape_sol, localInertiaSol );
body_sol = new btRigidBody(rbInfo_sol);
// Add the body to the dynamics world
myWorld->addRigidBody(body_sol);
// Create a clock for measuring time elapsed
sf::Clock montre;
//Variable pour calculer le delta de d�placement de la souris quand les clicks droit et gauche de la souris sont enfonc� pour manipuler la cam�ra
// Pour d�clancher la chute d'un seul kapla quand la touche Espace est relach�e
bool trigger = false;
unsigned int windowsWidth = Application.GetWidth();
unsigned int windowsHeight = Application.GetHeight();
int startPointX(0),startPointY(0);
float deltaX(0),deltaY(0),prevDeltaX(1),prevDeltaY(1);
int MouseX(0);
int MouseY(0);
bool show = true;
//unsigned int sizeWidth = sf::Window::GetWidth();
//On initialise une cam�ra qui sera plac� par d�faut par le constructeur
Camera camcam(110,60.0,60.0);
Cursor cursor;
bool test = false;
float time;
// pour avoir les infos clavier en temps r�el
const sf::Input& Input = Application.GetInput();
// Notre boucle d'affichage
while(Application.IsOpened() )
{
Application.ShowMouseCursor (false);
// r�f�rence vers l'entr�e associ�e � une fen�tre (pour r�cup�rer les donn�s clavier en temps r�el
sf::Event Event;
//Utilise les fl�che pour d�placer le Kapla qui va �tre lach�
// Get some useless input states, just to illustrate the tutorial
bool LeftKeyDown = Input.IsKeyDown(sf::Key::Left);
bool RightKeyDown = Input.IsKeyDown(sf::Key::Right);
bool UpKeyDown = Input.IsKeyDown(sf::Key::Up);
bool DownKeyDown = Input.IsKeyDown(sf::Key::Down);
bool RightButtonDown = Input.IsMouseButtonDown(sf::Mouse::Right);
bool LeftButtonDown = Input.IsMouseButtonDown(sf::Mouse::Left);
bool Espace = Input.IsKeyDown(sf::Key::Space);
bool Shift = Input.IsKeyDown(sf::Key::LShift);
MouseX = Input.GetMouseX() ;
MouseY = Input.GetMouseY() ;
if (LeftButtonDown)
{
if (show )
{
montre.Reset();
startPointY = MouseY;
startPointX = MouseX;
//cout<< "start point" <<startPointX<< endl;
show = false;
}
time = montre.GetElapsedTime();
//cout<< time << endl;
if (time > 0.02)
{
deltaX = ((MouseX-startPointX));
deltaY = ((MouseY-startPointY));
//show = true;
cout<< deltaX<< endl;
if ((prevDeltaX != deltaX)||(prevDeltaY != deltaY)) {
camcam.tumble(deltaX/5,deltaY/5);
show = true;
//montre.Reset();
prevDeltaX = deltaX;
prevDeltaY = deltaY;
}
}
}else
{
show = true;
cursor.set((MouseX - windowsWidth/2), (MouseY - windowsHeight/2));
}
if (Shift) {
test=true;
}else{
test=false;
}
if (LeftKeyDown) {
camcam.tumble(0.005, 0);
LeftKeyDown = false;
}
if (RightKeyDown) {
camcam.tumble(-0.005, 0);
RightKeyDown = false;
}
if (UpKeyDown) {
camcam.tumble(0, 0.005);
}
if (DownKeyDown) {
camcam.tumble(0, -0.005);
}
//unsigned int delta =mouseT3 - mouseT1;
//cout << delta<< endl;
while (Application.GetEvent(Event))
{
if (Event.Type == sf::Event::Resized)
glViewport(0, 0, Event.Size.Width, Event.Size.Height);
// Fen�tre ferm�e
if (Event.Type == sf::Event::Closed)
Application.Close();
// Touche 'echap' appuy�e
if ((Event.Type == sf::Event::KeyPressed) && (Event.Key.Code == sf::Key::Escape))
Application.Close();
if ((Event.Type == sf::Event::KeyReleased) && (Event.Key.Code == sf::Key::Space)) {
/// box ///////////////////////////////////////////
// create a shape
btCollisionShape* shape_kapla = new btBoxShape( btVector3(3,1,15) );
myTransform.setIdentity();
int dropX((MouseX - windowsWidth/2));
int dropY((MouseY - windowsHeight/2));
myTransform.setOrigin(btVector3(dropX,5,dropY));
btVector3 localInertia(0,0,0);
mass = 0.5f;
shape_kapla->calculateLocalInertia( mass, localInertia );
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
myMotionState = new btDefaultMotionState(myTransform);
btRigidBody::btRigidBodyConstructionInfo myBoxRigidBodyConstructionInfo( mass,myMotionState, shape_kapla, localInertia );
body_kapla = new btRigidBody(myBoxRigidBodyConstructionInfo);
//add the body to the dynamics world
myWorld->addRigidBody(body_kapla);
trigger=true;
}
}
Application.SetActive();
//NEED convertiseur coord souris -> coord du plan visible par la cam�ra
// le d�placement sur le plan de la fen�tre est proportionel � celui du plan de construction
// dessin le curseur
//cursor.drawKapla(1,15,3);
//touche espace enfonc�e et relach�e
if ((Espace))
{
}
if (myWorld)
{
myWorld->stepSimulation( 0.0001 );
}
camcam.display();
cursor.drawKapla(15, 3, 1);
// On recup�re la matrice OpenGL transform�e par Bullet qu'on appliquera � notre boite
if (trigger)
{
myMotionState->m_graphicsWorldTrans.getOpenGLMatrix( matrix );
glPushMatrix();
glMultMatrixf( matrix );
box(3,1,15);
glPopMatrix();
}
box(100,1,100);
// On a ffiche le sol;
if (test==true)
{
}
// swap buffers, etc
Application.Display();
}
}
int shpCallback(const char *path, const char *types, lo_arg **argv, int argc, void *data, void *user_data)
{
spin::spinApp &spin = spin::spinApp::Instance();
// make sure there is at least one argument (ie, a method to call):
if (!argc) return 1;
// get the method (argv[0]):
std::string theMethod;
if (lo_is_string_type((lo_type)types[0]))
{
theMethod = std::string((char *)argv[0]);
}
else return 1;
// parse the rest of the args:
std::vector<float> floatArgs;
std::vector<const char*> stringArgs;
for (int i=1; i<argc; i++)
{
if (lo_is_numerical_type((lo_type)types[i]))
{
floatArgs.push_back( (float) lo_hires_val((lo_type)types[i], argv[i]) );
} else {
stringArgs.push_back( (const char*) argv[i] );
}
}
// reset the transform
trans.setIdentity();
// update from spin state:
spin::ShapeNode *shp = dynamic_cast<spin::ShapeNode*>(spin.sceneManager->getNode("shp"));
if (shp)
{
btVector3 pos(shp->getTranslation().x(), shp->getTranslation().y(), shp->getTranslation().z());
btQuaternion quat(shp->getOrientationQuat().x(), shp->getOrientationQuat().y(), shp->getOrientationQuat().z(), shp->getOrientationQuat().w());
trans.setOrigin(pos);
trans.setRotation(quat);
}
// update state of physics object:
btRigidBody* shp_body = (btRigidBody*)user_data;
if (shp_body && shp)
{
if ((theMethod=="setTranslation") && (floatArgs.size()==3))
{
// update physics object:
std::cout << "got /shp setTranslation " <<floatArgs[0]<<","<<floatArgs[1]<<","<<floatArgs[2]<< std::endl;
btVector3 newpos(floatArgs[0],floatArgs[1],floatArgs[2]);
trans.setOrigin(newpos);
// CRASH:
// maybe we need to just save this somewhere and apply it during the update callback?
//shp_body->proceedToTransform(trans);
//shp_body->setWorldTransform(trans);
//shp_body->proceedToTransform(worldxform);
} else if ((theMethod=="setOrientation") && (floatArgs.size()==3))
{
osg::Quat q = osg::Quat( osg::DegreesToRadians(floatArgs[0]), osg::Vec3d(1,0,0),
osg::DegreesToRadians(floatArgs[1]), osg::Vec3d(0,1,0),
osg::DegreesToRadians(floatArgs[2]), osg::Vec3d(0,0,1));
btQuaternion quat(q.x(),q.y(),q.z(),q.w());
trans.setRotation(quat);
}
}
return 1;
}
