/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 * Created by:    Remi Ricard                                            *
 *                (remi.ricard@simlog.com or papaDoc@videotron.ca)       *
 * Creation date: 2007/05/04                                             *
 *************************************************************************/

/*
  This program demonstrates how the Piston joint works.

  A Piston joint enables the sliding of a body with respect to another body
  and the 2 bodies are free to rotate about the sliding axis.

  - The yellow body is fixed to the world.
  - The yellow body and the blue body are attached by a Piston joint with
    the axis along the x direction.
  - The purple object is a geometry obstacle.
  - The red line is the representation of the prismatic axis
  - The orange line is the representation of the rotoide axis
  - The light blue ball is the anchor position

  N.B. Many command options are available type -h to print them.
*/

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>
#include <math.h>
#include "texturepath.h"

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawSphere dsDrawSphereD
#endif


const dReal VEL_INC = 0.01; // Velocity increment

// physics parameters
const dReal PI = 3.14159265358979323846264338327950288419716939937510;
const dReal BODY1_LENGTH = 1.5;    // Size along the X axis

const dReal RADIUS = 0.2;
const dReal AXIS_RADIUS = 0.01;


#define X 0
#define Y 1
#define Z 2

enum INDEX
{
    BODY1 = 0,
    BODY2,
    RECT,
    BOX,
    OBS,
    GROUND,
    NUM_PARTS,
    ALL = NUM_PARTS
};

const int catBits[NUM_PARTS+1] =
{
    0x0001, ///< Ext Cylinder category
    0x0002, ///< Int Cylinder category
    0x0004, ///< Int_Rect Cylinder category
    0x0008, ///< Box category
    0x0010, ///< Obstacle category
    0x0020, ///< Ground category
    ~0L    ///< All categories
};

#define Mass1 10
#define Mass2 8


//camera view
static float xyz[3] = {2.0f,-3.5f,2.0000f};
static float hpr[3] = {90.000f,-25.5000f,0.0000f};


//world,space,body & geom
static dWorldID         world;
static dSpaceID         space;
static dJointGroupID    contactgroup;
static dBodyID          body[NUM_PARTS];
static dGeomID          geom[NUM_PARTS];

// Default Positions and anchor of the 2 bodies
dVector3 pos1;
dVector3 pos2;
dVector3 anchor;

static dJoint *joint;


const dReal BODY2_SIDES[3] = {0.4, 0.4, 0.4};
const dReal OBS_SIDES[3]  = {1,1,1};
const dReal RECT_SIDES[3] = {0.3, 0.1, 0.2};


int type = dJointTypePiston;

//#pragma message("tc to be changed to 0")

int tc = 0; // The test case choice;


//collision detection
static void nearCallback (void *, dGeomID o1, dGeomID o2)
{
    int i,n;

    dBodyID b1 = dGeomGetBody (o1);
    dBodyID b2 = dGeomGetBody (o2);
    if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return;
    const int N = 10;
    dContact contact[N];
    n = dCollide (o1,o2,N,&contact[0].geom,sizeof (dContact) );
    if (n > 0)
    {
        for  (i=0; i<n; i++)
        {
            contact[i].surface.mode = (dContactSlip1 | dContactSlip2 |
                                       dContactSoftERP | dContactSoftCFM |
                                       dContactApprox1);
            contact[i].surface.mu = 0.1;
            contact[i].surface.slip1 = 0.02;
            contact[i].surface.slip2 = 0.02;
            contact[i].surface.soft_erp = 0.1;
            contact[i].surface.soft_cfm = 0.0001;
            dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
            dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) );
        }
    }
}

static void printKeyBoardShortCut()
{
    printf ("Press 'h' for this help.\n");
    printf ("Press 'q' to add force on BLUE body along positive x direction.\n");
    printf ("Press 'w' to add force on BLUE body along negative x direction.\n");

    printf ("Press 'a' to add force on BLUE body along positive y direction.\n");
    printf ("Press 's' to add force on BLUE body along negative y direction.\n");

    printf ("Press 'z' to add force on BLUE body along positive z direction.\n");
    printf ("Press 'x' to add force on BLUE body along negative z direction.\n");

    printf ("Press 'e' to add torque on BLUE body around positive x direction \n");
    printf ("Press 'r' to add torque on BLUE body around negative x direction \n");

    printf ("Press 'd' to add torque on BLUE body around positive y direction \n");
    printf ("Press 'f' to add torque on BLUE body around negative y direction \n");

    printf ("Press 'c' to add torque on BLUE body around positive z direction \n");
    printf ("Press 'v' to add torque on BLUE body around negative z direction \n");

    printf ("Press 't' to add force on prismatic joint in the positive axis direction\n");
    printf ("Press 'y' to add force on prismatic joint in the negative axis direction\n");

    printf ("Press 'i' to add limits on the prismatic joint (0 to 0) \n");
    printf ("Press 'o' to add limits on the rotoide joint (0 to 0)\n");
    printf ("Press 'k' to add limits on the rotoide joint (-45 to 45deg) \n");
    printf ("Press 'l' to remove limits on the rotoide joint \n");


    printf ("Press '.' to increase joint velocity along the prismatic direction.\n");
    printf ("Press ',' to decrease joint velocity along the prismatic direction.\n");

    printf ("Press 'p' to print the Position of the joint.\n");

    printf ("Press '+' Go to the next test case.\n");
    printf ("Press '-' Go to the previous test case.\n");

    printf ("Press '8' To remove one of the body. The blue body and the world will be\n");
    printf ("          attached to the joint (blue body at position 1)\n");
    printf ("Press '9' To remove one of the body. The blue body and the world will be\n");
    printf ("          attached to the joint (body body at position 2)\n");


}


// start simulation - set viewpoint
static void start()
{
    dAllocateODEDataForThread(dAllocateMaskAll);

    dsSetViewpoint (xyz,hpr);
    printf ("This program demonstrates how the Piston joint works.\n");
    printf ("A Piston joint enables the sliding of a body with respect to another body\n");
    printf ("and the 2 bodies are free to rotate about the sliding axis.\n\n");
    printf ("The yellow body is fixed to the world\n");
    printf ("The yellow body and the blue body are attached by a Piston joint with\n");
    printf ("the axis along the x direction.\n");
    printf ("The purple object is a geometry obstacle.\n");

    printKeyBoardShortCut();
}


void setPositionBodies (int val)
{
    const dVector3 POS1 = {0,0,1.5,0};
    const dVector3 POS2 = {0,0,1.5,0};
    const dVector3 ANCHOR = {0,0,1.5,0};

    for (int i=0; i<3; ++i)
    {
        pos1[i] = POS1[i];
        pos2[i] = POS2[i];
        anchor[i] = ANCHOR[i];
    }

    if (body[BODY1])
    {
        dBodySetLinearVel (body[BODY1], 0,0,0);
        dBodySetAngularVel (body[BODY1], 0,0,0);
    }

    if (body[BODY2])
    {
        dBodySetLinearVel (body[BODY2], 0,0,0);
        dBodySetAngularVel (body[BODY2], 0,0,0);
    }

    switch (val)
    {
    case 3:
        pos1[Z] += -0.5;
        anchor[Z] -= 0.25;
        break;
    case 2:
        pos1[Z] -= 0.5;
        anchor[Z] -= 0.5;
        break;
    case 1:
        pos1[Z] += -0.5;
        break;
    default: // This is also case 0
        // Nothing to be done
        break;
    }

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

    if (body[BODY1])
    {
        dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]);
        dBodySetRotation (body[BODY1], R);
    }

    if (body[BODY2])
    {
        dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]);
        dBodySetRotation (body[BODY2], R);
    }



    if (joint)
    {
        joint->attach (body[BODY1], body[BODY2]);
        if (joint->getType() == dJointTypePiston)
            dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]);
    }

}


// function to update camera position at each step.
void update()
{
//   static FILE *file = fopen("x:/sim/src/libode/tstsrcSF/export.dat", "w");

//   static int cnt = 0;
//   char str[24];
//   sprintf(str, "%06d",cnt++);

//   dWorldExportDIF(world, file, str);
}


// called when a key pressed
static void command (int cmd)
{
    switch (cmd)
    {
    case 'h' :
    case 'H' :
    case '?' :
        printKeyBoardShortCut();
        break;

        // Force
    case 'q' :
    case 'Q' :
        dBodyAddForce (body[BODY1],4,0,0);
        break;
    case 'w' :
    case 'W' :
        dBodyAddForce (body[BODY1],-4,0,0);
        break;

    case 'a' :
    case 'A' :
        dBodyAddForce (body[BODY1],0,40,0);
        break;
    case 's' :
    case 'S' :
        dBodyAddForce (body[BODY1],0,-40,0);
        break;

    case 'z' :
    case 'Z' :
        dBodyAddForce (body[BODY1],0,0,4);
        break;
    case 'x' :
    case 'X' :
        dBodyAddForce (body[BODY1],0,0,-4);
        break;

        // Torque
    case 'e':
    case 'E':
        dBodyAddTorque (body[BODY1],0.1,0,0);
        break;
    case 'r':
    case 'R':
        dBodyAddTorque (body[BODY1],-0.1,0,0);
        break;

    case 'd':
    case 'D':
        dBodyAddTorque (body[BODY1],0, 0.1,0);
        break;
    case 'f':
    case 'F':
        dBodyAddTorque (body[BODY1],0,-0.1,0);
        break;

    case 'c':
    case 'C':
        dBodyAddTorque (body[BODY1],0.1,0,0);
        break;
    case 'v':
    case 'V':
        dBodyAddTorque (body[BODY1],-0.1,0,0);
        break;

    case 't':
    case 'T':
        if (joint->getType() == dJointTypePiston)
            dJointAddPistonForce (joint->id(),1);
        else
            dJointAddSliderForce (joint->id(),1);
        break;
    case 'y':
    case 'Y':
        if (joint->getType() == dJointTypePiston)
            dJointAddPistonForce (joint->id(),-1);
        else
            dJointAddSliderForce (joint->id(),-1);
        break;


    case '8' :
        dJointAttach(joint->id(), body[0], 0);
        break;
    case '9' :
        dJointAttach(joint->id(), 0, body[0]);
        break;

    case 'i':
    case 'I' :
        joint->setParam (dParamLoStop, 0);
        joint->setParam (dParamHiStop, 0);
        break;

    case 'o':
    case 'O' :
        joint->setParam (dParamLoStop2, 0);
        joint->setParam (dParamHiStop2, 0);
        break;

    case 'k':
    case 'K':
        joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
        joint->setParam (dParamHiStop2,  45.0*3.14159267/180.0);
        break;
    case 'l':
    case 'L':
        joint->setParam (dParamLoStop2, -dInfinity);
        joint->setParam (dParamHiStop2, dInfinity);
        break;

        // Velocity of joint
    case ',':
    case '<' :
    {
        dReal vel = joint->getParam (dParamVel) - VEL_INC;
        joint->setParam (dParamVel, vel);
        std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
    }
    break;

    case '.':
    case '>' :
    {
        dReal vel = joint->getParam (dParamVel) + VEL_INC;
        joint->setParam (dParamVel, vel);
        std::cout<<"Velocity = "<<vel<<"  FMax = 2"<<'\n';
    }
    break;

    case 'p' :
    case 'P' :
    {
        switch (joint->getType() )
        {
        case dJointTypeSlider :
        {
            dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
            std::cout<<"Position ="<<sj->getPosition() <<"\n";
        }
        break;
        case dJointTypePiston :
        {
            dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
            std::cout<<"Position ="<<rj->getPosition() <<"\n";
        }
        break;
        default:
        {} // keep the compiler happy
        }
    }
    break;

    case '+' :
        (++tc) %= 4;
        setPositionBodies (tc);
        break;
    case '-' :
        (--tc) %= 4;
        setPositionBodies (tc);
        break;


    }
}

static void drawBox (dGeomID id, int R, int G, int B)
{
    if (!id)
        return;

    const dReal *pos = dGeomGetPosition (id);
    const dReal *rot = dGeomGetRotation (id);
    dsSetColor (R,G,B);

    dVector3 l;
    dGeomBoxGetLengths (id, l);
    dsDrawBox (pos, rot, l);
}


// simulation loop
static void simLoop (int pause)
{
    const dReal *rot;
    dVector3 ax;
    dReal l=0;

    switch (joint->getType() )
    {
    case dJointTypeSlider :
        ( (dSliderJoint *) joint)->getAxis (ax);
        l = ( (dSliderJoint *) joint)->getPosition();
        break;
    case dJointTypePiston :
        ( (dPistonJoint *) joint)->getAxis (ax);
        l = ( (dPistonJoint *) joint)->getPosition();
        break;
    default:
    {} // keep the compiler happy
    }


    if (!pause)
    {
        double simstep = 0.01; // 1ms simulation steps
        double dt = dsElapsedTime();

        int nrofsteps = (int) ceilf (dt/simstep);
        if (!nrofsteps)
            nrofsteps = 1;

        for (int i=0; i<nrofsteps && !pause; i++)
        {
            dSpaceCollide (space,0,&nearCallback);
            dWorldStep (world, simstep);

            dJointGroupEmpty (contactgroup);
        }

        update();


        dReal radius, length;

        dsSetTexture (DS_WOOD);

        drawBox (geom[BODY2], 1,1,0);

        drawBox (geom[RECT], 0,0,1);

        if ( geom[BODY1] )
        {
            const dReal *pos = dGeomGetPosition (geom[BODY1]);
            rot = dGeomGetRotation (geom[BODY1]);
            dsSetColor (0,0,1);

            dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
            dsDrawCapsule (pos, rot, length, radius);
        }


        drawBox (geom[OBS], 1,0,1);


        // Draw the prismatic axis
        if ( geom[BODY1] )
        {
            const dReal *pos = dGeomGetPosition (geom[BODY1]);
            rot = dGeomGetRotation (geom[BODY2]);
            dVector3 p;
            p[X] = pos[X] - l*ax[X];
            p[Y] = pos[Y] - l*ax[Y];
            p[Z] = pos[Z] - l*ax[Z];
            dsSetColor (1,0,0);
            dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
        }


        if (joint->getType() == dJointTypePiston )
        {
            dVector3 anchor;
            dJointGetPistonAnchor(joint->id(), anchor);

            // Draw the rotoide axis
            rot = dGeomGetRotation (geom[BODY2]);
            dsSetColor (1,0.5,0);
            dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);


            dsSetColor (0,1,1);
            rot = dGeomGetRotation (geom[BODY1]);
            dsDrawSphere (anchor, rot, 1.5*RADIUS);
        }

    }
}


void Help (char **argv)
{
    printf ("%s ", argv[0]);
    printf (" -h | --help   : print this help\n");
    printf (" -s | --slider : Set the joint as a slider\n");
    printf (" -p | --piston : Set the joint as a Piston. (Default joint)\n");
    printf (" -1 | --offset1 : Create an offset between the 2 bodies\n");
    printf ("                  Offset one of the body by z=-0.5 and keep the anchor\n");
    printf ("                  point in the middle of the fixed body\n");
    printf (" -2 | --offset2 : Create an offset between the 2 bodies\n");
    printf ("                  Offset one of the body by z=-0.5 and set the anchor\n");
    printf ("                  point in the middle of the movable body\n");
    printf (" -3 | --offset3 : Create an offset between the 2 bodies\n");
    printf ("                  Offset one of the body by z=-0.5 and set the anchor\n");
    printf ("                  point in the middle of the 2 bodies\n");
    printf (" -t | --texture-path path  : Path to the texture.\n");
    printf ("                             Default = %s\n", DRAWSTUFF_TEXTURE_PATH);
    printf (" -n | --notFixed : In free space with no gravity mode");
    printf ("-notex          : Don't use texture\n");
    printf ("-noshadow       : No shadow\n");
    printf ("-noshadows      : No shadows\n");
    printf ("-pause          : Initial pause\n");
    printf ("--------------------------------------------------\n");
    printf ("Hit any key to continue:");
    getchar();

    exit (0);
}

int main (int argc, char **argv)
{
    dInitODE2(0);
    bool fixed  = true;

    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

    dVector3 offset;
    dSetZero (offset, 4);

    // Default test case

    if (argc >= 2 )
    {
        for (int i=1; i < argc; ++i)
        {
            //static int tata = 0;

            if (1)
            {
                if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
                    Help (argv);

                if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
                    type = dJointTypeSlider;

                if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
                {
                    int j = i+1;
                    if ( j >= argc      ||  // Check if we have enough arguments
                            argv[j][0] == '\0' ||  // We should have a path here
                            argv[j][0] == '-' ) // We should have a path not a command line
                        Help (argv);
                    else
                        fn.path_to_textures = argv[++i]; // Increase i since we use this argument
                }
            }


            if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
                tc = 1;

            if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
                tc = 2;

            if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
                tc = 3;

            if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
                fixed = false;
        }
    }

    world = dWorldCreate();
    dWorldSetERP (world, 0.8);

    space = dSimpleSpaceCreate (0);
    contactgroup = dJointGroupCreate (0);
    geom[GROUND] = dCreatePlane (space, 0,0,1,0);
    dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
    dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

    dMass m;
    dMatrix3 R;


    // Create the Obstacle
    geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
    dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
    dGeomSetCollideBits (geom[OBS], catBits[ALL]);
    //Rotation of 45deg around y
    dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
    dGeomSetRotation (geom[OBS], R);
    dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);


    //Rotation of 90deg around y
    // Will orient the Z axis along X
    dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);


    // Create Body2 (Wiil be attached to the world)
    body[BODY2] = dBodyCreate (world);
    // Main axis of cylinder is along X=1
    dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dMassAdjust (&m, Mass1);
    geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dGeomSetBody (geom[BODY2], body[BODY2]);
    dGeomSetOffsetRotation (geom[BODY2], R);
    dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
    dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
    dBodySetMass (body[BODY2], &m);


    // Create Body 1 (Slider on the prismatic axis)
    body[BODY1] = dBodyCreate (world);
    // Main axis of capsule is along X=1
    dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
    dMassAdjust (&m, Mass1);
    geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
    dGeomSetBody (geom[BODY1], body[BODY1]);
    dGeomSetOffsetRotation (geom[BODY1], R);
    dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
    dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);

    dMass mRect;
    dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dMassAdd (&m, &mRect);
    // TODO: translate m?
    geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dGeomSetBody (geom[RECT], body[BODY1]);
    dGeomSetOffsetPosition (geom[RECT],
                            (BODY1_LENGTH-RECT_SIDES[0]) /2.0,
                            0.0,
                            -RADIUS -RECT_SIDES[2]/2.0);
    dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
    dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );

    dBodySetMass (body[BODY1], &m);



    setPositionBodies (tc);


    if ( fixed )
    {
        // Attache external cylinder to the world
        dJointID fixed = dJointCreateFixed (world,0);
        dJointAttach (fixed , NULL, body[BODY2]);
        dJointSetFixed (fixed );
        dWorldSetGravity (world,0,0,-0.8);
    }
    else
    {
        dWorldSetGravity (world,0,0,0);
    }




    // The static is here only to help debugging
    switch (type)
    {
    case dJointTypeSlider :
    {
        dSliderJoint *sj = new dSliderJoint (world, 0);
        sj->attach (body[BODY1], body[BODY2]);
        sj->setAxis (1, 0, 0);
        joint = sj;
    }
    break;

    case dJointTypePiston : // fall through default
    default:
    {
        dPistonJoint *pj = new dPistonJoint (world, 0);
        pj->attach (body[BODY1], body[BODY2]);
        pj->setAxis (1, 0, 0);

        dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);

        joint = pj;
    }
    break;
    };


    // run simulation
    dsSimulationLoop (argc,argv,400,300,&fn);

    delete joint;
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}