int test_dBoxTouchesBox()
{
  int k,bt1,bt2;
  dVector3 p1,p2,side1,side2;
  dMatrix3 R1,R2;

  dSimpleSpace space(0);
  dGeomID box1 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box1);
  dGeomID box2 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box2);

  dMakeRandomVector (p1,3,0.5);
  dMakeRandomVector (p2,3,0.5);
  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);

  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
  dGeomSetRotation (box1,R1);
  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
  dGeomSetRotation (box2,R2);
  draw_all_objects (space);

  int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
  int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
  bt1 = t1 || t2;
  bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);

  if (bt1 != bt2) FAILED();

  /*
    // some more debugging info if necessary
    if (bt1 && bt2) printf ("agree - boxes touch\n");
    if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
    if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
			     "says no\n");
    if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
			     "says yes\n");
  */

  PASSED();
}

void RigidBodyEnvironment::createWorld(void)
{
    // BEGIN SETTING UP AN OPENDE ENVIRONMENT
    // ***********************************

    bodyWorld = dWorldCreate();
    space = dHashSpaceCreate(0);

    dWorldSetGravity(bodyWorld, 0, 0, -0.981);

    double lx = 0.2;
    double ly = 0.2;
    double lz = 0.1;

    dMassSetBox(&m, 1, lx, ly, lz);

    boxGeom = dCreateBox(space, lx, ly, lz);
    boxBody = dBodyCreate(bodyWorld);
    dBodySetMass(boxBody, &m);
    dGeomSetBody(boxGeom, boxBody);

    // *********************************
    // END SETTING UP AN OPENDE ENVIRONMENT

    setPlanningParameters();
}

void TrackedVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    //dMassAdjust(&this->vehicleMass, 2.40);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);

    this->leftTrack->create();
    this->rightTrack->create();

    dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
    dRigidBodyArraySetPosition(leftTrack->bodyArray,  -wheelBase/2, -(w - trackWidth)/2, 0);
    dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2,  (w - trackWidth)/2, 0);

    this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
    this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
    dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
    dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
    dJointSetFixed(this->leftTrackJoint);
    dJointSetFixed(this->rightTrackJoint);

    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
    dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}

void createInvisibleHead( float* pos )
{
	dMatrix3 head_orientation;
	dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);

	//position and orientation
	head.Body = dBodyCreate(World);
	dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
	dBodySetRotation(head.Body, head_orientation);
	dBodySetLinearVel(head.Body, 0, 0, 0);
	dBodySetData(head.Body, (void *)0);

	//mass
	dMass head_mass;
	dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
	dBodySetMass(head.Body, &head_mass);

	//geometry
	head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
	dGeomSetBody(head.Geom, head.Body);

	//fixed joint
	invis_box_joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(invis_box_joint, body.Body, head.Body);
	dJointSetFixed(invis_box_joint);
}

void Balaenidae::embody(dWorldID world, dSpaceID space)
{
    me = dBodyCreate(world);
    embody(me);
    geom = dCreateBox( space, 100.0f, 40, 500.0f);   // scale 50
    dGeomSetBody(geom, me);
}

 BarrelGeom2(Barrel *b) :
     OdeGeom(dCreateBox(gDynamicSpace, b->radius_ * 1.4f, b->radius_ * 1.4f, b->height_)),
     b_(b)
 {
     dGeomSetBody(id_, b->body_->id_);
     dGeomSetData(id_, this);
 }

void TSRODERigidBody::AddBoxGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, const TSRVector3& _vExtents, float _fDensity )
{
	TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
    dMass totalMass;
    dBodyGetMass( m_BodyID, &totalMass );
    if ( m_GeomIDs.size() == 0 )
    {
        dMassSetZero( &totalMass );
    }
    dMatrix4 R;
    dVector3 P;
    Matrix4ToODE( _bodyToGeomTransform, R, P );
    dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
    dGeomID encapsulatedGeom = 0;
    dMass currMass;
    dMassSetZero( &currMass );
    
    encapsulatedGeom = dCreateBox( 0, _vExtents.x, _vExtents.y, _vExtents.z );
    dMassSetBox( &currMass, _fDensity, _vExtents.x,_vExtents.y,_vExtents.z);
    dMassRotate( &currMass, R);
    //dMassTranslate(&currMass,P[0],P[1],P[2]);
    dMassAdd( &totalMass, &currMass );
    dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
    dGeomSetRotation( encapsulatedGeom, R );
    dGeomTransformSetCleanup( geomTransform, 1 );
    dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
    dGeomSetBody( geomTransform, m_BodyID );
    m_GeomIDs.push_back( geomTransform );
    dBodySetMass( m_BodyID, &totalMass );
}

void Robots::construirChassi(dWorldID world)
{
    for (int i=0; i < 2; i++)
    {
        // Cria objeto e geometria
        this->body[i] = dBodyCreate(world);
        this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);

        // Define a posição do objeto
        dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);

		// Se o robô for do segundo time, deve ser rotacionado em 180 graus
        if ((this->id == 3) || (this->id == 4) || (this->id == 5))
        {
			dQuaternion q;
			dQFromAxisAndAngle(q,0,0,1,M_PI);
			dBodySetQuaternion(this->body[i],q);
		}

        // Define a massa do objeto
        dMass m;
        dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT);	// O segundo bloco é mais curto
        dMassAdjust(&m,CMASS*(1+i*2));					// O segundo bloco é mais pesado
        dBodySetMass(this->body[i],&m);
                
        // Associa o objeto à sua geometria
        dGeomSetBody(this->box[i],this->body[i]);
    }
		
    // O chassis é composto por dois blocos que são fixos entre si
    dJointID fixed = dJointCreateFixed(world,0);
    dJointAttach(fixed,this->body[1],this->body[0]);
    dJointSetFixed(fixed); 
}

dGeomID BoxGeometry::createGeometry(dSpaceID space)
{
  Geometry::createGeometry(space);
  approxRadius = std::min(std::min(depth, width), height) * 0.5f;
  approxRadiusSqr = approxRadius * approxRadius;
  return dCreateBox(space, depth, width, height);
}

void	CPHActivationShape::Create(const Fvector start_pos,const Fvector start_size,IPhysicsShellHolder* ref_obj,EType _type/*=etBox*/,u16	flags)
{
	VERIFY(ref_obj);
	R_ASSERT(_valid( start_pos ) );
	R_ASSERT( _valid( start_size ) );

	m_body			=	dBodyCreate	(0)												;
	dMass m;
	dMassSetSphere(&m,1.f,100000.f);
	dMassAdjust(&m,1.f);
	dBodySetMass(m_body,&m);
	switch(_type)
	{
	case etBox:
	m_geom			=	dCreateBox	(0,start_size.x,start_size.y,start_size.z)		;
	break;

	case etSphere:
	m_geom			=	dCreateSphere	(0,start_size.x);
	break;
	};

	dGeomCreateUserData				(m_geom)										;
	dGeomUserDataSetObjectContactCallback(m_geom,ActivateTestDepthCallback)			;
	dGeomUserDataSetPhysicsRefObject(m_geom,ref_obj)								;
	dGeomSetBody					(m_geom,m_body)									;
	dBodySetPosition				(m_body,start_pos.x,start_pos.y,start_pos.z)	;
	Island()		.AddBody		(m_body)										;
	dBodyEnable						(m_body)										;
	m_safe_state					.create(m_body)									;
	spatial_register				()												;
	m_flags.set(flags,TRUE);
}

void BGcubeD_D::BGSetboxBandG() {
    
    
    
    boxGeom_m = dCreateBox (gODESpace, 1,1,1);
    
}

CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
    body = dBodyCreate(world);
    geom = dCreateBox(space, sides[0], sides[1], sides[2]);
    dGeomSetBody(geom, body);
    dGeomSetData(geom, this);
    dMass mass;
    mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
    dBodySetMass(body, &mass);

    const dMatrix3 rotationMatrix = {1,0,0,0,
                                     0,1,0,0,
                                     0,0,1,0};

    dBodySetRotation(body, rotationMatrix);

    dBodySetPosition(body,x,y,z);

    for(int i = 0 ; i < 6 ; i++)
        attachedPieces[i] = NULL; // initialize attached piece array to all null pointers

    color[0] = 1;
    color[1] = 1;
    color[2] = 1;
}

GameObject::GameObject(GameWorld& gw, const ObjectPrototype& proto, double x, double y, double z, int id) :
	m_meshName(proto.m_meshName), m_sceneEntity(nullptr), m_sceneNode(nullptr),	m_lockRotation(proto.m_lockRotation), m_isKinematic(proto.m_isKinematic),
	m_maxTurn(proto.m_maxTurnAngle), m_maxForward(proto.m_maxForward), m_maxBackward(proto.m_maxBackward),
	m_hitPoints(proto.m_maxHitPoints), m_collisionAccum(0), m_totalDamage(0), m_hasAgent(proto.m_hasAgent), m_gw(gw), m_id(id), m_oldGS(nullptr), m_newGS(nullptr)
{
	m_sceneEntity = gw.GetScene()->createEntity(gw.GetMesh(m_meshName));
	m_sceneNode = gw.GetScene()->getRootSceneNode()->createChildSceneNode();
	m_sceneNode->attachObject(m_sceneEntity);

	Ogre::Vector3 size = m_sceneEntity->getBoundingBox().getSize();

	m_body = dBodyCreate(gw.GetPhysicsWorld());
	m_geom = dCreateBox(gw.GetPhysicsSpace(), size.x, size.y, size.z);
	dMassSetBox(&m_mass, proto.m_density, size.x, size.y, size.z);
	dBodySetMass(m_body, &m_mass);
	dGeomSetBody(m_geom, m_body);
	dBodySetPosition(m_body, x, y, z);
	
	// automagically disable things when the body is still for long enough
	dBodySetAutoDisableFlag(m_body, 1);
	dBodySetAutoDisableLinearThreshold(m_body, 0.1f);
	dBodySetAutoDisableAngularThreshold(m_body, 0.25f);
	dBodySetAutoDisableSteps(m_body, 1);

	// improve simulation accuracy
	dBodySetDamping(m_body, 0.0f, 0.1f);

	if (proto.m_registerCollisions) {
		gw.RegisterForCollisions(this);
	}

	if (proto.m_isKinematic) {
		dBodySetKinematic(m_body);
	}
}

void PhysicsActor::postLoad(){

	dMass m;

    if (type==CUBESHAPE)
      {
	  geom = dCreateBox(space,shape.x,shape.y,shape.z);
      dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
      }
	if (type==CAPSULESHAPE)
	  {
	  geom = dCreateCapsule(space,shape.x,shape.y);
      dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y);      //the '3' means align on z-axis  and density is shape.z
	  generateCapsuleList();
	  }
	dMassAdjust(&m,mass);
	dBodySetMass(body,&m);


	dGeomSetBody(geom,body);

    //initialise position
    if (base){
        Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix;
        dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]);
    }else{
        dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]);
    }

   dBodySetDamping(body, linearDamp, angleDamp);

   bInit=true;

}

void cubeD_D::SetboxBandG() {
    
    boxBody_m = dBodyCreate (gODEWorld);
    
    boxGeom_m = dCreateBox (gODESpace, 1,1,1);
    
}

    void box::create_physical_body(
                    double x, 
                    double y, 
                    double z, 
                    double size_x, 
                    double size_y, 
                    double size_z, 
                    double mass, 
                    manager& mgr)
    {
         world_id = mgr.ode_world();
         space_id = mgr.ode_space();

        //create and position the geom to represent the pysical shape of the rigid body   
        geom_id = dCreateBox (mgr.ode_space(), size_x, size_y, size_z);
        object::set_geom_data(geom_id);
        dGeomSetPosition (geom_id, x, y, z); 		
		size[0] = size_x;
        size[1] = size_y;
        size[2] = size_z;

		if(mass > 0)
		{
			object::create_rigid_body(x, y, z, mgr);
			set_mass(mass);
			dGeomSetBody (geom_id, body_id);  
		}
    }

OscPrismODE::OscPrismODE(dWorldID odeWorld, dSpaceID odeSpace, const char *name, OscBase *parent)
    : OscPrism(NULL, name, parent)
{
    dGeomID odeGeom = dCreateBox(odeSpace, m_size.x, m_size.y, m_size.z);

    m_pSpecial = new ODEObject(this, odeGeom, odeWorld, odeSpace);
    m_density.set(m_density.m_value);
}

void ComponentPhysicsGeom::createGeomBox()
{
	ASSERT(physicsEngine, "\"physicsEngine\" was null");
	geom = dCreateBox(physicsEngine->getSpace(),
	                  collisionRadius,
					  collisionRadius,
					  desiredHeight);
}

//===========================================================================
void cODEGenericBody::createDynamicBox(const double a_lengthX,
									   const double a_lengthY,
									   const double a_lengthZ,
                                       bool a_staticObject,
									   const cVector3d& a_offsetPos,
									   const cMatrix3d& a_offsetRot)
{
    // create ode dynamic body if object is non static
    if (!a_staticObject)
    {
        m_ode_body = dBodyCreate(m_ODEWorld->m_ode_world);

	    // store pointer to current object
	    dBodySetData (m_ode_body, this);
    }
    m_static = a_staticObject;

    // build box
    m_ode_geom = dCreateBox(m_ODEWorld->m_ode_space, a_lengthX, a_lengthY, a_lengthZ);

	// adjust position offset
	dGeomSetPosition (m_ode_geom, a_offsetPos.x, a_offsetPos.y, a_offsetPos.z);

	// adjust orientation offset
	dMatrix3 R;
	R[0]  = a_offsetRot.m[0][0];
	R[1]  = a_offsetRot.m[0][1];
	R[2]  = a_offsetRot.m[0][2];
	R[4]  = a_offsetRot.m[1][0];
	R[5]  = a_offsetRot.m[1][1];
	R[6]  = a_offsetRot.m[1][2];
	R[8]  = a_offsetRot.m[2][0];
	R[9]  = a_offsetRot.m[2][1];
	R[10] = a_offsetRot.m[2][2];
	dGeomSetRotation (m_ode_geom, R);

    // set inertia properties
    if (!m_static)
    {
        dMassSetBox(&m_ode_mass, 1.0, a_lengthX, a_lengthY, a_lengthZ);
	    dMassAdjust(&m_ode_mass, m_mass);
	    dBodySetMass(m_ode_body,&m_ode_mass);

        // attach body and geometry together
        dGeomSetBody(m_ode_geom, m_ode_body);
    }

    // store dynamic model type
    m_typeDynamicCollisionModel = ODE_MODEL_BOX;

    // store dynamic model parameters
    m_paramDynColModel0 = a_lengthX;
    m_paramDynColModel1 = a_lengthY;
    m_paramDynColModel2 = a_lengthZ;
    m_posOffsetDynColModel = a_offsetPos;
    m_rotOffsetDynColModel = a_offsetRot;
}

void LaserBeam::embody(dWorldID world, dSpaceID space)
{
    me = dBodyCreate(world);
    embody(me);
    //geom = dCreateSphere( space, 2.64f);
    geom = dCreateBox( space, Gunshot::width, Gunshot::height, Gunshot::length);
    dBodySetGravityMode(me,0);
    dGeomSetBody(geom, me);
}