#include "nitrito.h"
void Coche::draw()
{
int i,j;
for(j=1;j<5;j++)
{
setTransform (dBodyGetPosition(body[j]),dBodyGetRotation(body[j]));
for(i=0; i<rueda.NumTriangle; i++)
{
glBindTexture( GL_TEXTURE_2D, texture[rueda.TriangleArray[i].Texture]);
//glDisable(GL_TEXTURE_2D);
//float color[] = { 1.0f, 1.0f, 1.0f, 1.0f };
//glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, color);
glBegin(GL_TRIANGLES);
glNormal3f(rueda.NormalArray[rueda.TriangleArray[i].Normal[0]].X,
rueda.NormalArray[rueda.TriangleArray[i].Normal[0]].Y,
rueda.NormalArray[rueda.TriangleArray[i].Normal[0]].Z);
glTexCoord2f(rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[0]].U,
rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[0]].V);
glVertex3f(rueda.VertexArray[rueda.TriangleArray[i].Vertex[0]].X,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[0]].Y,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[0]].Z);
glNormal3f(rueda.NormalArray[rueda.TriangleArray[i].Normal[1]].X,
rueda.NormalArray[rueda.TriangleArray[i].Normal[1]].Y,
rueda.NormalArray[rueda.TriangleArray[i].Normal[1]].Z);
glTexCoord2f(rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[1]].U,
rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[1]].V);
glVertex3f(rueda.VertexArray[rueda.TriangleArray[i].Vertex[1]].X,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[1]].Y,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[1]].Z);
glNormal3f(rueda.NormalArray[rueda.TriangleArray[i].Normal[2]].X,
rueda.NormalArray[rueda.TriangleArray[i].Normal[2]].Y,
rueda.NormalArray[rueda.TriangleArray[i].Normal[2]].Z);
glTexCoord2f(rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[2]].U,
rueda.TexCoordArray[rueda.TriangleArray[i].TexCoord[2]].V);
glVertex3f(rueda.VertexArray[rueda.TriangleArray[i].Vertex[2]].X,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[2]].Y,
rueda.VertexArray[rueda.TriangleArray[i].Vertex[2]].Z);
glEnd();
}
glPopMatrix();
setTransform (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]));
for(i=0; i<chasis.NumTriangle; i++)
{
glBindTexture( GL_TEXTURE_2D, texture[chasis.TriangleArray[i].Texture]);
glBegin(GL_TRIANGLES);
glNormal3f(chasis.NormalArray[chasis.TriangleArray[i].Normal[0]].X,
chasis.NormalArray[chasis.TriangleArray[i].Normal[0]].Y,
chasis.NormalArray[chasis.TriangleArray[i].Normal[0]].Z);
glTexCoord2f(chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[0]].U,
chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[0]].V);
glVertex3f(chasis.VertexArray[chasis.TriangleArray[i].Vertex[0]].X,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[0]].Y,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[0]].Z);
glNormal3f(chasis.NormalArray[chasis.TriangleArray[i].Normal[1]].X,
chasis.NormalArray[chasis.TriangleArray[i].Normal[1]].Y,
chasis.NormalArray[chasis.TriangleArray[i].Normal[1]].Z);
glTexCoord2f(chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[1]].U,
chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[1]].V);
glVertex3f(chasis.VertexArray[chasis.TriangleArray[i].Vertex[1]].X,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[1]].Y,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[1]].Z);
glNormal3f(chasis.NormalArray[chasis.TriangleArray[i].Normal[2]].X,
chasis.NormalArray[chasis.TriangleArray[i].Normal[2]].Y,
chasis.NormalArray[chasis.TriangleArray[i].Normal[2]].Z);
glTexCoord2f(chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[2]].U,
chasis.TexCoordArray[chasis.TriangleArray[i].TexCoord[2]].V);
glVertex3f(chasis.VertexArray[chasis.TriangleArray[i].Vertex[2]].X,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[2]].Y,
chasis.VertexArray[chasis.TriangleArray[i].Vertex[2]].Z);
glEnd();
}
glPopMatrix();
}
}
void Coche::create()
{
int i;
dMass m;
length = 1.4; // chassis length
width = 0.8; // chassis width
height = 0.7; // chassis height
radius = 0.21; // wheel radius
startz = 0.6; // starting height of chassis
cmass = 2; // chassis mass
wmass = 3; // wheel mass
tracc = TRACC_DEL; // traccion
power = 2.3; // potensia
for(i=0;i<world->map.area;i++) {
if(world->map.data[i]==OBJ_SALIDA)
{
startx=world->object[i].posx;
starty=world->object[i].posy;
break;
}
}
// chassis body
body[0] = dBodyCreate (world->world_id);
dBodySetPosition (body[0],startx,starty,startz);
dMassSetBox (&m,1,length*1.2,width*1.2,height);
dMassTranslate (&m, 0, 0, -0.3);
dMassAdjust (&m,cmass);
dBodySetMass (body[0],&m);
box[0] = dCreateBox (0,length,width,height);
dGeomSetBody (box[0],body[0]);
//dReal sides[3] = { 0.2, 0.2 ,0.2};
//body[5] = dBodyCreate (world->world_id);
//dBodySetPosition (body[5],2,0,4);
//dMassSetSphere (&m,5,sides[0]);
//dMassAdjust (&m,CMASS);
//pelota = dCreateSphere (world->space,sides[0]);
//dGeomSetBody (pelota,body[5]);
// wheel bodies
for (i=1; i<=4; i++) {
body[i] = dBodyCreate (world->world_id);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[i],q);
dMassSetSphere (&m,1,radius);
dMassAdjust (&m,wmass);
dBodySetMass (body[i],&m);
dBodySetFiniteRotationMode (body[i], 1);
sphere[i-1] = dCreateSphere (0,radius);
dGeomSetBody (sphere[i-1],body[i]);
}
dBodySetPosition (body[1],startx+0.3*length,starty+width*0.5,startz-height*0.55);
dBodySetPosition (body[4],startx+0.3*length,starty-width*0.5,startz-height*0.55);
dBodySetPosition (body[2],startx-0.3*length,starty+width*0.5,startz-height*0.55);
dBodySetPosition (body[3],startx-0.3*length,starty-width*0.5,startz-height*0.55);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[i] = dJointCreateHinge2 (world->world_id,0);
dJointAttach (joint[i],body[0],body[i+1]);
const dReal *a = dBodyGetPosition (body[i+1]);
dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[i],0,0,1);
dJointSetHinge2Axis2 (joint[i],0,1,0);
}
// set joint suspension
for (i=0; i<4; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.1);
}
// lock back wheels along the steering axis
for (i=0; i<4; i++) {
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (joint[i],dParamLoStop,0);
dJointSetHinge2Param (joint[i],dParamHiStop,0);
}
// create car space and add it to the top level space
car_space = dSimpleSpaceCreate (world->space);
dSpaceSetCleanup (car_space,0);
dSpaceAdd (car_space,box[0]);
dSpaceAdd (car_space,sphere[0]);
dSpaceAdd (car_space,sphere[1]);
dSpaceAdd (car_space,sphere[2]);
dSpaceAdd (car_space,sphere[3]);
LoaderClass.LoadObj(add_data_path("models/chasis.obj"));
chasis = LoaderClass.ReturnObj();
LoaderClass.LoadObj(add_data_path("models/rueda.obj"));
rueda = LoaderClass.ReturnObj();
}
void Coche::destroy()
{
dGeomDestroy (box[0]);
dGeomDestroy (sphere[0]);
dGeomDestroy (sphere[1]);
dGeomDestroy (sphere[2]);
dGeomDestroy (sphere[3]);
}
void Coche::move()
{
int i;
// Segun la formula magica del libro gordo de petete
//ERP = h kp / (h kp + kd)
//CFM = 1 / (h kp + kd)
float kp = 200;
float kd = 10;
float h = world->step_size;
for (i=0; i<4; i++) {
dJointSetHinge2Param (joint[i],dParamSuspensionERP,h*kp/(h*kp+kd));
dJointSetHinge2Param (joint[i],dParamSuspensionCFM,1/(h*kp+kd));
}
// motor
if(speed>0)
{
if(tracc==TRACC_DEL || tracc==TRACC_4x4)
{
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,power*1.1);
dJointSetHinge2Param (joint[3],dParamVel2,-speed);
dJointSetHinge2Param (joint[3],dParamFMax2,power*1.1);
}
if(tracc==TRACC_TRS || tracc==TRACC_4x4)
{
dJointSetHinge2Param (joint[1],dParamVel2,-speed);
dJointSetHinge2Param (joint[1],dParamFMax2,power*0.9);
dJointSetHinge2Param (joint[2],dParamVel2,-speed);
dJointSetHinge2Param (joint[2],dParamFMax2,power*0.9);
}
}
else if(speed<0)
{
if(tracc==TRACC_DEL || tracc==TRACC_4x4)
{
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,power*1.4);
dJointSetHinge2Param (joint[3],dParamVel2,-speed);
dJointSetHinge2Param (joint[3],dParamFMax2,power*1.4);
}
if(tracc==TRACC_TRS || tracc==TRACC_4x4)
{
dJointSetHinge2Param (joint[1],dParamVel2,-speed);
dJointSetHinge2Param (joint[1],dParamFMax2,power*2);
dJointSetHinge2Param (joint[2],dParamVel2,-speed);
dJointSetHinge2Param (joint[2],dParamFMax2,power*2);
}
}
else if(speed==0)
{
dJointSetHinge2Param (joint[1],dParamVel2,0);
dJointSetHinge2Param (joint[1],dParamFMax2,0.2);
dJointSetHinge2Param (joint[2],dParamVel2,0);
dJointSetHinge2Param (joint[2],dParamFMax2,0.2);
dJointSetHinge2Param (joint[0],dParamVel2,0);
dJointSetHinge2Param (joint[0],dParamFMax2,0.2);
dJointSetHinge2Param (joint[3],dParamVel2,0);
dJointSetHinge2Param (joint[3],dParamFMax2,0.2);
}
if(brake!=0)
{
dJointSetHinge2Param (joint[1],dParamVel2,0);
dJointSetHinge2Param (joint[1],dParamFMax2,brake);
dJointSetHinge2Param (joint[2],dParamVel2,0);
dJointSetHinge2Param (joint[2],dParamFMax2,brake);
}
// velocimetro
dReal const * cv = dBodyGetLinearVel( body[0] );
vel = sqrt(cv[0]*cv[0] + cv[1]*cv[1] + cv[2]*cv[2]);
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
if(vel/4>=1) v *= 10.0/(vel/4);
else v *= 10.0;
dJointSetHinge2Param (joint[0],dParamVel,v);
dJointSetHinge2Param (joint[0],dParamFMax,1.5);
dJointSetHinge2Param (joint[0],dParamLoStop,-0.7);
dJointSetHinge2Param (joint[0],dParamHiStop,0.7);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.7);
// steering
v = steer - dJointGetHinge2Angle1 (joint[3]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
if(vel/4>=1) v *= 10.0/(vel/4);
else v *= 10.0;
dJointSetHinge2Param (joint[3],dParamVel,v);
dJointSetHinge2Param (joint[3],dParamFMax,1.5);
dJointSetHinge2Param (joint[3],dParamLoStop,-0.7);
dJointSetHinge2Param (joint[3],dParamHiStop,0.7);
dJointSetHinge2Param (joint[3],dParamFudgeFactor,0.7);
// steering
//revolucionometro (tacometro?)
dReal const * crev1 = dBodyGetAngularVel( body[1] );
dReal const * crev2 = dBodyGetAngularVel( body[2] );
dReal const * crev3 = dBodyGetAngularVel( body[3] );
dReal const * crev4 = dBodyGetAngularVel( body[4] );
rev = ( sqrt(crev1[0]*crev1[0] + crev1[1]*crev1[1] + crev1[2]*crev1[2])+
sqrt(crev2[0]*crev2[0] + crev2[1]*crev2[1] + crev2[2]*crev2[2])+
sqrt(crev3[0]*crev3[0] + crev3[1]*crev3[1] + crev3[2]*crev3[2])+
sqrt(crev4[0]*crev4[0] + crev4[1]*crev4[1] + crev4[2]*crev4[2]))/4;
//aerodinamica
dBodyAddForce( body[0], 0.1*-cv[0],0.1*-cv[1],0.1*-cv[2] );
const dReal *pos = dBodyGetPosition(body[0]);
x = pos[0];
y = pos[1];
z = pos[2];
xs = cv[0];
ys = cv[1];
zs = cv[2];
if(variable_global == 0) {
struct Vector3{
float x,y,z;
};
Vector3 bodyPoint;
Vector3 hingePoint;
Vector3 axis;
float displacement;
for( int ix = 0; ix < 4; ++ix ) {
dJointGetHinge2Anchor2( joint[ix], &bodyPoint.x );
dJointGetHinge2Anchor( joint[ix], &hingePoint.x );
dJointGetHinge2Axis1( joint[ix], &axis.x );
displacement = (hingePoint.x - bodyPoint.x) / axis.x;
float amt = displacement * 1;
if( displacement > 0 ) {
if( amt > 15 ) {
amt = 15;
}
dBodyAddForce( body[ix+1], -axis.x * amt, -axis.y * amt, -axis.z * amt );
dReal const * wp = dBodyGetPosition( body[ix+1] );
dBodyAddForceAtPos( body[0], axis.x*amt, axis.y*amt, axis.z*amt, wp[0], wp[1], wp[2] );
dBodyAddForce( body[(ix+1)], axis.x * amt, axis.y * amt, axis.z * amt );
wp = dBodyGetPosition( body[ix+1] );
dBodyAddForceAtPos( body[0], -axis.x*amt, -axis.y*amt, -axis.z*amt, wp[0], wp[1], wp[2] );
}
}
}
}