inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
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;
}