//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createHeightFieldShapeFromSerialization(std::string _filename, const Math::Matrix4& _transform)
{
std::cout << "\nLoading TreeCollision from file " << _filename.c_str() << std::endl;
FILE* file;
file = fopen(_filename.c_str(), "rb");
if( file==0 )
{
std::cout << "Error: could not open file " << _filename.c_str() << " for loading terrain." << std::endl;
throw Zen::Utility::runtime_exception("Zen::ZBullet::PhysicsZone::createHeightFieldShapeFromSerialization - Could not Open File!");
}
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
//dynamic_cast<CollisionShape*>(pShape.get())->setShapePtr(NewtonCreateTreeCollision(m_pZone, NULL));
//dynamic_cast<CollisionShape*>(pShape.get())->setShapePtr(NewtonCreateTreeCollisionFromSerialization(m_pZone, NULL, ZBulletDeSerializeFile, file));
fclose (file);
std::cout << "Finished loading TreeCollision from file." << std::endl;
/* this is not description logic
// Note: collision is deleted in attachBody() so do not use it after this function call.
if (!attachBody((btCollisionShape *)pShape->getShapePtr()))
return false;
btRigidBodySetMatrix(pShape->getActorPtr(), _transform.m_array);
return true;
*/
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createCapsuleShape(float _radius, float _height)
{
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
pRawPointer->setShapePtr(new btCapsuleShape(_radius, _height));
// TODO - set offsetMatrix (currently passing NULL, which centers the body at its origin)
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createBoxShape(float _dx, float _dy, float _dz)
{
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
pRawPointer->setShapePtr(new btBoxShape(btVector3(_dx,_dy,_dz)));
// TODO - set offsetMatrix (currently passing NULL, which centers the body at its origin)
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createTreeCollisionShape(std::string _filename)
{
// TODO impl
std::cout << "Error: TreeCollision physics shape not yet implemented." << std::endl;
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
//collision = pShape->setShapePtr(NewtonCreateTreeCollision(dynamic_cast<PhysicsZone*>(m_pZone.get())->getZonePtr(), ...));
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createOvoidShape(float _radiusX, float _radiusY, float _radiusZ)
{
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
//seem to recall something about a multisphere shape being scalable in multiple directions... research further
pRawPointer->setShapePtr(new btSphereShape(btScalar(_radiusX)));
// TODO - set offsetMatrix (currently passing NULL, which centers the body at its origin)
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createNullShape()
{
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
//bit of a hacktastic workaround, but see http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4461&hilit=btRigidBody+vs+btCollisionObject for intent
pRawPointer->setShapePtr(new btSphereShape(btScalar(1.)));
pRawPointer->setIsNUllShape(true);
// TODO - set offsetMatrix (currently passing NULL, which centers the body at its origin)
return pShape;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
PhysicsZone::pCollisionShape_type
PhysicsZone::createHeightFieldShapeFromRaw(std::string _filename, size_t _size, float _maxHeight, float _scaleXY, const Math::Matrix4& _transform, bool _bSerialize)
{
CollisionShape* pRawPointer = new CollisionShape();
pCollisionShape_type pShape(pRawPointer, boost::bind(&PhysicsZone::destroyCollisionShape, this, _1));
// TODO - verify the file is a RAW file
FILE* file;
file = fopen(_filename.c_str(), "rb");
if( file== NULL )
{
std::cout << "Error: could not open file " << _filename.c_str() << "in CollisionShape::initHeightFieldShape()." << std::endl;
throw Zen::Utility::runtime_exception("Zen::ZBullet::PhysicsZone::createHeightFieldShapeFromRaw - Could not Open File!");
}
// read the RAW file into a heightfield array
// TODO - don't assume this is a 16-bit RAW file
Zen::Math::Real heightScaleFactor = 65536.0f / _maxHeight;
// TODO - change to managed pointer:
Zen::Math::Real* pHeightFieldArray = new Zen::Math::Real[_size * _size];
size_t row, column;
for (row = 0; row < _size; row++)
{
U16 height = 0;
for (column = 0; column < _size; column++)
{
fread(&height, sizeof(U16), 1, file);
pHeightFieldArray[column + (row * _size)] = height;
}
}
fclose(file);
//btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges)
dynamic_cast<CollisionShape*>(pShape.get())->setShapePtr(new btHeightfieldTerrainShape(_scaleXY,_scaleXY,&pHeightFieldArray,_maxHeight,0,true,false));
// std::cout << "\nTreeCollision finished assigning vertices.\nNow beginning mesh optimization...\n\nPlease be patient, as this may take a few MINUTES depending on your computer." << std::endl;
delete pHeightFieldArray;
return pShape;
}
void PhysicsEngine::RegisterObject(ModelNode *pItem){
/*********************************************************************
*ADDING SHAPES
**********************************************************************/
if (dynamic_cast<Shape*>(pItem) != NULL) {
Shape* pNodeShape = (Shape*) pItem;
/************************************
*ADDING A RAYCAST VEHICLE
************************************/
if(dynamic_cast<SimRaycastVehicle*>(pNodeShape)!=NULL){
bullet_vehicle btRayVehicle( pItem, dynamics_world_.get(),
vehicle_raycaster_);
CollisionShapePtr pShape( btRayVehicle.getBulletShapePtr() );
MotionStatePtr pMotionState( btRayVehicle.getBulletMotionStatePtr() );
RigidBodyPtr body( btRayVehicle.getBulletBodyPtr() );
VehiclePtr vehicle( btRayVehicle.getBulletRaycastVehicle() );
std::shared_ptr<Vehicle_Entity> pEntity( new Vehicle_Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
pEntity->m_pVehicle = vehicle;
ray_vehicles_map_[pItem->GetName()] = pEntity;
}
//Box
if (dynamic_cast<BoxShape*>( pNodeShape ) != NULL) {
bullet_cube btBox(pItem);
CollisionShapePtr pShape( btBox.getBulletShapePtr() );
MotionStatePtr pMotionState( btBox.getBulletMotionStatePtr() );
RigidBodyPtr body( btBox.getBulletBodyPtr() );
dynamics_world_->addRigidBody( body.get() );
//Save the object; easier deconstruction this way.
std::shared_ptr<Entity> pEntity( new Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
shapes_map_[pItem->GetName()] = pEntity;
}
//Cylinder
else if (dynamic_cast<CylinderShape*>( pNodeShape ) != NULL) {
bullet_cylinder btCylinder(pItem);
CollisionShapePtr pShape( btCylinder.getBulletShapePtr() );
MotionStatePtr pMotionState( btCylinder.getBulletMotionStatePtr() );
RigidBodyPtr body( btCylinder.getBulletBodyPtr() );
dynamics_world_->addRigidBody( body.get() );
//Save the object; easier deconstruction this way.
std::shared_ptr<Entity> pEntity( new Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
shapes_map_[pItem->GetName()] = pEntity;
}
//Plane
else if (dynamic_cast<PlaneShape*>( pNodeShape ) != NULL) {
bullet_plane btPlane(pItem);
CollisionShapePtr pShape( btPlane.getBulletShapePtr() );
MotionStatePtr pMotionState( btPlane.getBulletMotionStatePtr() );
RigidBodyPtr body( btPlane.getBulletBodyPtr() );
dynamics_world_->addRigidBody( body.get() );
//Save the object; easier deconstruction this way.
std::shared_ptr<Entity> pEntity( new Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
shapes_map_[pItem->GetName()] = pEntity;
}
//Heightmap
else if (dynamic_cast<HeightmapShape*>( pNodeShape ) != NULL) {
bullet_heightmap btMap(pItem);
CollisionShapePtr pShape( btMap.getBulletShapePtr() );
MotionStatePtr pMotionState( btMap.getBulletMotionStatePtr() );
RigidBodyPtr body( btMap.getBulletBodyPtr() );
dynamics_world_->addRigidBody( body.get() );
//Save the object; easier deconstruction this way.
std::shared_ptr<Entity> pEntity( new Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
shapes_map_[pItem->GetName()] = pEntity;
}
//Mesh
else if (dynamic_cast<MeshShape*>( pNodeShape ) != NULL){
bullet_mesh btMesh(pItem);
CollisionShapePtr pShape( btMesh.getBulletShapePtr() );
MotionStatePtr pMotionState( btMesh.getBulletMotionStatePtr() );
RigidBodyPtr body( btMesh.getBulletBodyPtr() );
dynamics_world_->addRigidBody( body.get() );
//Save the object; easier deconstruction this way.
std::shared_ptr<Entity> pEntity( new Entity );
pEntity->m_pRigidBody = body;
pEntity->m_pShape = pShape;
pEntity->m_pMotionState = pMotionState;
shapes_map_[pItem->GetName()] = pEntity;
}
}
/*********************************************************************
*ADDING CONSTRAINTS
**********************************************************************/
else if (dynamic_cast<Constraint*>(pItem) != NULL) {
Constraint* pNodeCon = (Constraint*) pItem;
// Point to Point
if (dynamic_cast<PToPOne*>( pNodeCon ) != NULL) {
PToPOne* pCon = (PToPOne*) pNodeCon;
btRigidBody* RigidShape_A;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
btVector3 pivot_A(pCon->m_pivot_in_A[0], pCon->m_pivot_in_A[1],
pCon->m_pivot_in_A[2]);
btPoint2PointConstraint* PToP =
new btPoint2PointConstraint(*RigidShape_A, pivot_A);
dynamics_world_->addConstraint(PToP);
ptop_map_[pCon->GetName()] = PToP;
}
else if(dynamic_cast<PToPTwo*>( pNodeCon ) != NULL) {
PToPTwo* pCon = (PToPTwo*) pNodeCon;
btRigidBody* RigidShape_A;
btRigidBody* RigidShape_B;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
if(isVehicle(pCon->m_Shape_B)){
std::shared_ptr<Vehicle_Entity> Shape_B = ray_vehicles_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_B = shapes_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
btVector3 pivot_A(pCon->m_pivot_in_A[0], pCon->m_pivot_in_A[1],
pCon->m_pivot_in_A[2]);
btVector3 pivot_B(pCon->m_pivot_in_B[0], pCon->m_pivot_in_B[1],
pCon->m_pivot_in_B[2]);
btPoint2PointConstraint* PToP =
new btPoint2PointConstraint(*RigidShape_A, *RigidShape_B,
pivot_A, pivot_B);
dynamics_world_->addConstraint(PToP);
ptop_map_[pCon->GetName()] = PToP;
}
//Hinge
else if(dynamic_cast<HingeOnePivot*>( pNodeCon ) != NULL) {
HingeOnePivot* pCon = (HingeOnePivot*) pNodeCon;
btRigidBody* RigidShape_A;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
btVector3 pivot_A(pCon->m_pivot_in_A[0], pCon->m_pivot_in_A[1],
pCon->m_pivot_in_A[2]);
btVector3 axis_A(pCon->m_axis_in_A[0], pCon->m_axis_in_A[1],
pCon->m_axis_in_A[2]);
btHingeConstraint* Hinge =
new btHingeConstraint(*RigidShape_A, pivot_A,
axis_A, true);
Hinge->setLimit(pCon->m_low_limit, pCon->m_high_limit, pCon->m_softness,
pCon->m_bias, pCon->m_relaxation);
dynamics_world_->addConstraint(Hinge);
hinge_map_[pCon->GetName()] = Hinge;
}
else if(dynamic_cast<HingeTwoPivot*>( pNodeCon ) != NULL) {
HingeTwoPivot* pCon = (HingeTwoPivot*) pNodeCon;
btRigidBody* RigidShape_A;
btRigidBody* RigidShape_B;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
if(isVehicle(pCon->m_Shape_B)){
std::shared_ptr<Vehicle_Entity> Shape_B = ray_vehicles_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_B = shapes_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
btVector3 pivot_A(pCon->m_pivot_in_A[0], pCon->m_pivot_in_A[1],
pCon->m_pivot_in_A[2]);
btVector3 axis_A(pCon->m_axis_in_A[0], pCon->m_axis_in_A[1],
pCon->m_axis_in_A[2]);
btVector3 pivot_B(pCon->m_pivot_in_B[0], pCon->m_pivot_in_B[1],
pCon->m_pivot_in_B[2]);
btVector3 axis_B(pCon->m_axis_in_B[0], pCon->m_axis_in_B[1],
pCon->m_axis_in_B[2]);
btHingeConstraint* Hinge =
new btHingeConstraint(*RigidShape_A,
*RigidShape_B,
pivot_A, pivot_B,
axis_A, axis_B,
true);
Hinge->setLimit(pCon->m_low_limit, pCon->m_high_limit, pCon->m_softness,
pCon->m_bias, pCon->m_relaxation);
dynamics_world_->addConstraint(Hinge);
hinge_map_[pCon->GetName()] = Hinge;
}
//Hinge2
else if(dynamic_cast<Hinge2*>( pNodeCon ) != NULL) {
Hinge2* pCon = (Hinge2*) pNodeCon;
btRigidBody* RigidShape_A;
btRigidBody* RigidShape_B;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
if(isVehicle(pCon->m_Shape_B)){
std::shared_ptr<Vehicle_Entity> Shape_B = ray_vehicles_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_B = shapes_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
btVector3 btAnchor(pCon->m_Anchor[0], pCon->m_Anchor[1],
pCon->m_Anchor[2]);
btVector3 btAxis_1(pCon->m_Axis_1[0], pCon->m_Axis_1[1],
pCon->m_Axis_1[2]);
btVector3 btAxis_2(pCon->m_Axis_2[0], pCon->m_Axis_2[1],
pCon->m_Axis_2[2]);
btHinge2Constraint* Hinge =
new btHinge2Constraint(*RigidShape_A, *RigidShape_B,
btAnchor, btAxis_1, btAxis_2);
Hinge->setAngularLowerLimit(toBulletVec3(pCon->m_LowerAngLimit));
Hinge->setAngularUpperLimit(toBulletVec3(pCon->m_UpperAngLimit));
Hinge->setLinearLowerLimit(toBulletVec3(pCon->m_LowerLinLimit));
Hinge->setLinearUpperLimit(toBulletVec3(pCon->m_UpperLinLimit));
Hinge->enableSpring(3, true);
Hinge->setStiffness(3, pCon->m_stiffness);
Hinge->setDamping(3, pCon->m_damping);
dynamics_world_->addConstraint(Hinge);
hinge2_map_[pCon->GetName()] = Hinge;
}
//SixDOF
else if(dynamic_cast<SixDOFOne*>( pNodeCon ) != NULL) {
SixDOFOne* pCon = (SixDOFOne*) pNodeCon;
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
btTransform trans_A = toBullet(_Cart2T(pCon->m_Transform_A));
btGeneric6DofConstraint* SixDOF =
new btGeneric6DofConstraint(*Shape_A->m_pRigidBody.get(),
trans_A,
true);
SixDOF->setLinearLowerLimit(toBulletVec3(pCon->m_LowerLinLimit));
SixDOF->setLinearUpperLimit(toBulletVec3(pCon->m_UpperLinLimit));
SixDOF->setAngularLowerLimit(toBulletVec3(pCon->m_LowerAngLimit));
SixDOF->setAngularUpperLimit(toBulletVec3(pCon->m_UpperAngLimit));
dynamics_world_->addConstraint(SixDOF);
sixdof_map_[pCon->GetName()] = SixDOF;
}
else if(dynamic_cast<SixDOFTwo*>( pNodeCon ) != NULL) {
SixDOFTwo* pCon = (SixDOFTwo*) pNodeCon;
btRigidBody* RigidShape_A;
btRigidBody* RigidShape_B;
if(isVehicle(pCon->m_Shape_A)){
std::shared_ptr<Vehicle_Entity> Shape_A = ray_vehicles_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_A = shapes_map_.at(pCon->m_Shape_A);
RigidShape_A = Shape_A->m_pRigidBody.get();
}
if(isVehicle(pCon->m_Shape_B)){
std::shared_ptr<Vehicle_Entity> Shape_B = ray_vehicles_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
else{
std::shared_ptr<Entity> Shape_B = shapes_map_.at(pCon->m_Shape_B);
RigidShape_B = Shape_B->m_pRigidBody.get();
}
btTransform trans_A = toBullet(_Cart2T(pCon->m_Transform_A));
btTransform trans_B = toBullet(_Cart2T(pCon->m_Transform_B));
btGeneric6DofConstraint* SixDOF =
new btGeneric6DofConstraint(*RigidShape_A, *RigidShape_B,
trans_A, trans_B,
true);
SixDOF->setLinearLowerLimit(toBulletVec3(pCon->m_LowerLinLimit));
SixDOF->setLinearUpperLimit(toBulletVec3(pCon->m_UpperLinLimit));
SixDOF->setAngularLowerLimit(toBulletVec3(pCon->m_LowerAngLimit));
SixDOF->setAngularUpperLimit(toBulletVec3(pCon->m_UpperAngLimit));
dynamics_world_->addConstraint(SixDOF);
sixdof_map_[pCon->GetName()] = SixDOF;
}
}
return;
}
