/*************************************************************************
 *                                                                       *
 * 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.                     *
 *                                                                       *
 *************************************************************************/

/*

*/


#include 
#include 

#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif


// some constants

#define LENGTH 0.8	// chassis length
#define WIDTH 0.5	// chassis width
#define HEIGHT 0.3	// chassis height
#define RADIUS 0.125	// wheel radius
#define STARTZ 0.5	// starting height of chassis
#define CMASS 1		// chassis mass
#define WMASS 0.2	// wheel mass


// dynamics and collision objects (chassis, 3 wheels, environment)

static dWorldID world;
static dSpaceID space;
static dBodyID body[7];
static dBodyID upperBody[6];
static dBodyID secondCarBody[7];
static dBodyID secondCarUpperBody[6];
static dJointID joint[6];
static dJointID upperjoint[6];	// joint[0] is the right front wheel
static dJointID secondCarJoint[6];
static dJointID secondCarUpperJoint[6];
//thirdCarAdded
static dBodyID thirdCarBody[13];
static dJointID thirdCarJoint[12];
//
static dUniversalJoint carLink;
static dUniversalJoint carLink2;//3
static dJointGroupID contactgroup;
static dGeomID ground,geom_group;
static dGeomID box[1];
static dGeomID box2[1];
static dGeomID box3[1];
static dGeomID sphere[6];
static dGeomID upperSphere[6];
static dGeomID sphere2[6];
static dGeomID upperSphere2[6];
static dGeomID sphere3[12];//3
static dGeomID ground_box;


// things that the user controls

static dReal rlspeed=0, llspeed=0, lspeed=0, uspeed=0,steer=0;
static dReal ruspeed=0,luspeed=0,steer2=0,steer3=0,motor=0, motor2=0;	// user commands

void firstCar_init();
void secondCar_init();
void thirdCar_init();
void firstLink_init();
void secondLink_init();
void firstCar_loop();
void secondCar_loop();
void thirdCar_loop();

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i,n;

  // only collide things with the ground
  int g1 = (o1 == ground || o1 == ground_box);
  int g2 = (o2 == ground || o2 == ground_box);
  if (!(g1 ^ g2)) 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
     Source: geocities.com/usnacrew