// Create a ball and a pole
void createBallandPole() {
	dMass m1;
	dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;

	// ball
	ball.radius = 0.2;
	ball.mass = 1.0;
	ball.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetSphereTotal(&m1, ball.mass, ball.radius);
	dBodySetMass(ball.body, &m1);
	dBodySetPosition(ball.body, x0, y0, z0);

	ball.geom = dCreateSphere(space, ball.radius);
	dGeomSetBody(ball.geom, ball.body);

	// pole
	pole.radius = 0.025;
	pole.length = 1.0;
	pole.mass = 1.0;
	pole.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
	dBodySetMass(pole.body, &m1);
	dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);

	pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
	dGeomSetBody(pole.geom, pole.body);

	// hinge joint
	joint = dJointCreateHinge(world, 0);
	dJointAttach(joint, ball.body, pole.body);
	dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
	dJointSetHingeAxis(joint, 1, 0, 0);
}

void constructWorldForTest (dReal gravity, int bodycount,
 /* body 1 pos */           dReal pos1x, dReal pos1y, dReal pos1z,
 /* body 2 pos */           dReal pos2x, dReal pos2y, dReal pos2z,
 /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
 /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
 /* rotation angles */      dReal a1, dReal a2)
{
  // create world
  world = dWorldCreate();
  dWorldSetERP (world,0.2);
  dWorldSetCFM (world,1e-6);
  dWorldSetGravity (world,0,0,gravity);

  dMass m;
  dMassSetBox (&m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&m,MASS);

  body[0] = dBodyCreate (world);
  dBodySetMass (body[0],&m);
  dBodySetPosition (body[0], pos1x, pos1y, pos1z);
  dQuaternion q;
  dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
  dBodySetQuaternion (body[0],q);

  if (bodycount==2) {
    body[1] = dBodyCreate (world);
    dBodySetMass (body[1],&m);
    dBodySetPosition (body[1], pos2x, pos2y, pos2z);
    dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
    dBodySetQuaternion (body[1],q);
  }
  else body[1] = 0;
}

WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
    dBodyCreate(world);
    body = dBodyCreate(world);

    radius = .35;
    thickness = .25;
    activationDirection = 0;

    geom = dCreateCylinder(space, radius, thickness);
    dGeomSetBody(geom, body);
    dGeomSetData(geom, this);
    dMass mass;
    mass.setCylinder(.5, 1, radius, thickness);
    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);

    attachmentOffset = thickness + .25;

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

int main (int argc, char **argv)
{
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.stop = 0;
    fn.command = 0;
    fn.path_to_textures = "../../drawstuff/textures";
 
    dInitODE ();
    // create world
    world = dWorldCreate ();
    space = dHashSpaceCreate (0);
    dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
    dWorldSetCFM (world,1e-5);
    dCreatePlane (space,0,0,1,0);
    contactgroup = dJointGroupCreate (0);

    // create object
    sphere0 = dBodyCreate (world);
    sphere0_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere0,&m);
    dGeomSetBody (sphere0_geom,sphere0);
 
    sphere1 = dBodyCreate (world);
    sphere1_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere1,&m);
    dGeomSetBody (sphere1_geom,sphere1);
 
    sphere2 = dBodyCreate (world);
    sphere2_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere2,&m);
    dGeomSetBody (sphere2_geom,sphere2);
  
    // set initial position
    dBodySetPosition (sphere0,0,0,4);
    dBodySetPosition (sphere1,5,0,4);
    dBodySetPosition (sphere2,-2,0,4);

// run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);
    // clean up
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}

int main (int argc, char **argv)
{
    // set for drawing
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = NULL;
    fn.stop    = NULL;
    fn.path_to_textures = "../textures";


    dInitODE();              // init ODE
    world = dWorldCreate();  // create a dynamic world
    dWorldSetGravity(world,0,0,-0.1);

    dMass m;                 // a parameter for mass
    dMassSetZero (&m);       // initialize the parameter

    //@a sphere
    sphere.body = dBodyCreate (world);     // create a rigid body
    dReal radius = 0.5;                    // radius [m]
    dMassSetSphere (&m,DENSITY,radius);    // calculate a mass parameter for a sphere
    dBodySetMass (sphere.body,&m);         // set the mass parameter to the body
    dBodySetPosition (sphere.body,0,1, 1); // set the position of the body


    //@a box
    box.body = dBodyCreate (world);
    dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
    dBodySetMass (box.body,&m);
    dBodySetPosition (box.body,0,2,1);

    // a capsule
    capsule.body = dBodyCreate (world);
    dMassSetCapsule(&m,DENSITY,3,radius,length);
    dBodySetMass (capsule.body,&m);
    dBodySetPosition (capsule.body,0,4,1);

    // a cylinder
    cylinder.body = dBodyCreate (world);
    dMassSetCylinder(&m,DENSITY,3,radius,length);
    dBodySetMass (cylinder.body,&m);
    dBodySetPosition (cylinder.body,0,3,1);

    // do the simulation
    dsSimulationLoop (argc,argv,960,480,&fn);

    dWorldDestroy (world); // destroy the world
    dCloseODE();           // close ODE

    return 0;
}

    void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
    {
      math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);

      rotation.LoadIdentity();
      rotation.SetFromAngles(math::DegreesToRadians(rot));

      const math::cMat4 m = rotation.GetMatrix();

      dMatrix3 r;
      r[0] = m[0];    r[1] = m[4];    r[2] = m[8];    r[3] = 0;
      r[4] = m[1];    r[5] = m[5];    r[6] = m[9];    r[7] = 0;
      r[8] = m[2];    r[9] = m[6];    r[10] = m[10];  r[11] = 0;

      position = pos;

      dGeomSetPosition(geom, position.x, position.y, position.z);
      dGeomSetRotation(geom, r);

      if (bBody) {
        body = dBodyCreate(pWorld->GetWorld());
        dBodySetPosition(body, position.x, position.y, position.z);
        dBodySetRotation(body, r);
        dBodySetAutoDisableFlag(body, 1);

        dGeomSetBody(geom, body);

        pWorld->AddPhysicsObject(shared_from_this());
      }
    }

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);
}

/*
=================================================================================
createFixedLeg

	Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
	ODEObject &bodyAttachedTo,
	dJointID& joint,
	dReal xPos, dReal yPos, dReal zPos,
	dReal xRot, dReal yRot, dReal zRot,
	dReal radius,
	dReal length)
{
	dMatrix3 legOrient;
	dRFromEulerAngles(legOrient, xRot, yRot, zRot);

	//position and orientation
	leg.Body = dBodyCreate(World);
	dBodySetPosition(leg.Body, xPos, yPos, zPos);
	dBodySetRotation(leg.Body, legOrient);
	dBodySetLinearVel(leg.Body, 0, 0, 0);
	dBodySetData(leg.Body, (void *)0);

	//mass
	dMass legMass;
	dMassSetCapsule(&legMass, 1, 3, radius, length);
	dBodySetMass(leg.Body, &legMass);

	//geometry
	leg.Geom = dCreateCapsule(Space, radius, length);
	dGeomSetBody(leg.Geom, leg.Body);

	//fixed joint
	joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
	dJointSetFixed(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);
}

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;
}

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);
}

int main(int argc, char** argv)
{
    init_gfx();
    init_2d();
    
    world = dWorldCreate();

    dBodyID ball = dBodyCreate(world);
    dBodySetPosition(ball, 0, 0, 0);

    for (;;) {
        Uint8* chars = SDL_GetKeyState(NULL);

        if (chars[SDLK_LEFT]) {
            dBodyAddForce(ball, -force, 0, 0);
        }
        if (chars[SDLK_RIGHT]) {
            dBodyAddForce(ball, +force, 0, 0);
        }
        if (chars[SDLK_UP]) {
            dBodyAddForce(ball, 0, +force, 0);
        }
        if (chars[SDLK_DOWN]) {
            dBodyAddForce(ball, 0, -force, 0);
        }

        const dReal* pos = dBodyGetPosition(ball);
        glTranslatef(pos[0], pos[1], pos[2]);
        draw_circle();

        dWorldQuickStep(world, 0.01);
        step();
    }
}

void ODERigidObject::Create(dWorldID worldID,dSpaceID space,bool useBoundaryLayer)
{
  Clear(); 
  spaceID = space;
  bodyID = dBodyCreate(worldID);

  dMass mass;
  mass.mass = obj.mass;
  //NOTE: in ODE, COM must be zero vector! -- we take care of this by shifting geometry
  //CopyVector3(mass.c,obj.com);
  mass.c[0] = mass.c[1] = mass.c[2] = 0; mass.c[3] = 1.0;
  CopyMatrix(mass.I,obj.inertia);
  int res=dMassCheck(&mass);
  if(res != 1) {
    fprintf(stderr,"Uh... mass is not considered to be valid by ODE?\n");
    std::cerr<<"Inertia: "<<obj.inertia<<std::endl;
  }
  dBodySetMass(bodyID,&mass);
  
  geometry = new ODEGeometry;
  geometry->Create(&obj.geometry,spaceID,-obj.com,useBoundaryLayer);
  dGeomSetBody(geometry->geom(),bodyID);
  dGeomSetData(geometry->geom(),(void*)-1);
  geometry->SetPadding(defaultPadding);
  geometry->surf().kRestitution = obj.kRestitution;
  geometry->surf().kFriction = obj.kFriction;
  geometry->surf().kStiffness = obj.kStiffness;
  geometry->surf().kDamping = obj.kDamping;

  SetTransform(obj.T);
}

App::App(dWorldID world, dSpaceID space, AppDesignID app_design,
	 BladeDesignID blade_design, dBodyID main_body)
:  MyObject(world, space), ebMain(main_body)
{
	// body count:  9 = blade + 2 * ( piston(2) + piston(2) )
	// geom count:  10 = body_num + 1
	// joint count: 12 = 2 * (A,B,C,D,E + piston )
	Init(9, 12, 10);	// b j g

	bBlade = dBodyCreate(world);
	gBlade = blade_design->createGeom(space);

	int i;

	for (i = 0; i < 2; ++i) {
		app_design->setSide(i);	// tell it which side i want
		half[i] = new AppHalf(app_design);
		
		// connect the half to the body and to the blade
		dBodyID B = half[i]->getBBody();
		dBodyID B_star = half[i]->getBstarBody();
		dBodyID E = half[i]->getEBody();

		BJoint = dCreate
	}

	
	return;
}

ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace)
    : m_odeWorld(odeWorld), m_odeSpace(odeSpace)
{
    m_object = obj;

    m_odeGeom = odeGeom;
    m_odeBody = NULL;
    m_odeBody = dBodyCreate(m_odeWorld);

    assert(m_odeGeom!=NULL);

    dBodySetPosition(m_odeBody, 0, 0, 0);
    dGeomSetPosition(m_odeGeom, 0, 0, 0);

    // note: owners must override this by setting the density. can't
    //       do it here because obj->m_pSpecial is not yet
    //       initialized.
    dMassSetSphere(&m_odeMass, 1, 1);
    dBodySetMass(m_odeBody, &m_odeMass);

    dGeomSetBody(m_odeGeom, m_odeBody);
    dGeomSetData(m_odeGeom, obj);

    if (!obj) return;

    obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this);
    obj->m_position.setSetCallback(ODEObject::on_set_position, this);
    obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this);
    obj->m_accel.setSetCallback(ODEObject::on_set_accel, this);
    obj->m_force.setSetCallback(ODEObject::on_set_force, this);

    obj->addHandler("push", "ffffff", ODEObject::push_handler);
}

PhysicsActor::PhysicsActor(){

name="PhysicsActor";
drawType=DRAW_CUBE;

world=renderer->physicsWorld;
space=renderer->collisionSpace;

body = dBodyCreate(renderer->physicsWorld);
geom=0;
joint=NULL;
oldBase=NULL;

shape=Vector3f(0.1,0.3,0.1);
mass=1.0;
type=CAPSULESHAPE;
//drawType=DRAW_SPECIAL;
bJointedToBase=false;
bSkeletalPart=false;
bFixToWorld=false;
bInit=false;

angleDamp=0.59;
linearDamp=0.004;

registerProperties();



menuType.push_back("12AssignButton");
menuProp.push_back("NULL");
menuIcon.push_back("icon_base");
menuName.push_back("make joint");
}

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);
	}
}

RigidBody::RigidBody(const dWorldID &_world, const ngl::Vec3 &_pos)
{
  m_world=_world;
  m_id=dBodyCreate(m_world);
  dBodySetPosition(m_id,_pos.m_x,_pos.m_y,_pos.m_z);
  enable();
}