void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
void CProtoHapticDoc::SimulationStep()
{
int elapsed= clock() - m_lastSimStep;
int steps= (int)((((float)elapsed)*m_simSpeed)*0.1f);
if(steps<1) {
m_lastSimStep= clock();
return;
}
// collision detection
for(int i= 0; i<m_shapeCount; i++) {
if(m_shapes[i]->isCollisionDynamic())
dGeomSetBody (m_geoms[i], bodies[i]);
else
dGeomSetBody (m_geoms[i], 0);
}
dSpaceCollide (m_spaceID,this,&nearCallbackStatic);
dWorldQuickStep (m_worldID, steps);
dJointGroupEmpty (m_jointGroup);
for( int i= 0; i<m_shapeCount; i++) {
//air resistance
if(m_shapes[i]->isCollisionDynamic()||m_shapes[i]->isProxyDynamic()) {
const dReal *vel;
const dReal *angvel;
vel= dBodyGetLinearVel (bodies[i]);
dBodyAddForce (bodies[i], -vel[0]*m_airResistance, -vel[1]*m_airResistance, -vel[2]*m_airResistance);
angvel= dBodyGetAngularVel (bodies[i]);
dBodyAddTorque (bodies[i], -angvel[0]*m_airResistance, -angvel[1]*m_airResistance, -angvel[2]*m_airResistance);
}
HHLRC rc= hlGetCurrentContext();
// proxy0
hlMakeCurrent(m_context);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
if(((CProtoHapticApp*)AfxGetApp())->isTwoDevices()) {
// proxy1
hlMakeCurrent(m_context_1);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching1()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
}
hlMakeCurrent(rc);
// gravity
if(m_shapes[i]->isCollisionDynamic())
dBodyAddForce(bodies[i], 0.0, -m_shapes[i]->getMass()*(m_gravity/10.0f), 0.0);
const dReal *pos;
const dReal *rot;
pos= dBodyGetPosition (bodies[i]);
rot= dBodyGetRotation (bodies[i]);
float rotation[16];
rotation[0]= rot[0]; rotation[4]= rot[1]; rotation[8]= rot[2]; rotation[12]= rot[3];
rotation[1]= rot[4]; rotation[5]= rot[5]; rotation[9]= rot[6]; rotation[13]= rot[7];
rotation[2]= rot[8]; rotation[6]= rot[9]; rotation[10]= rot[10]; rotation[14]= rot[11];
rotation[3]= 0.0; rotation[7]= 0.0; rotation[11]= 0.0; rotation[15]= 1.0;
m_shapes[i]->setRotation(rotation);
m_shapes[i]->setLocation(pos[0], pos[1], pos[2]);
}
m_lastSimStep= clock();
}
void resetSimulation()
{
int i;
i = 0;
// destroy world if it exists
if (bodies)
{
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
}
for (i = 0; i < 1000; i++)
wb_stepsdis[i] = 0;
// recreate world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-1.5);
dWorldSetCFM (world, 1e-5);
dWorldSetERP (world, 0.8);
dWorldSetQuickStepNumIterations (world,ITERS);
ground = dCreatePlane (space,0,0,1,0);
bodies = 0;
joints = 0;
boxes = 0;
spheres = 0;
wb = 0;
#ifdef CARS
for (dReal x = 0.0; x < COLS*(LENGTH+RADIUS); x += LENGTH+RADIUS)
for (dReal y = -((ROWS-1)*(WIDTH/2+RADIUS)); y <= ((ROWS-1)*(WIDTH/2+RADIUS)); y += WIDTH+RADIUS*2)
makeCar(x, y, bodies, joints, boxes, spheres);
#endif
#ifdef WALL
bool offset = false;
for (dReal z = WBOXSIZE/2.0; z <= WALLHEIGHT; z+=WBOXSIZE)
{
offset = !offset;
for (dReal y = (-WALLWIDTH+z)/2; y <= (WALLWIDTH-z)/2; y+=WBOXSIZE)
{
wall_bodies[wb] = dBodyCreate (world);
dBodySetPosition (wall_bodies[wb],-20,y,z);
dMassSetBox (&m,1,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dMassAdjust (&m, WALLMASS);
dBodySetMass (wall_bodies[wb],&m);
wall_boxes[wb] = dCreateBox (space,WBOXSIZE,WBOXSIZE,WBOXSIZE);
dGeomSetBody (wall_boxes[wb],wall_bodies[wb]);
//dBodyDisable(wall_bodies[wb++]);
wb++;
}
}
dMessage(0,"wall boxes: %i", wb);
#endif
#ifdef BALLS
for (dReal x = -7; x <= -4; x+=1)
for (dReal y = -1.5; y <= 1.5; y+=1)
for (dReal z = 1; z <= 4; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,x*RADIUS*2,y*RADIUS*2,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, BALLMASS);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef ONEBALL
b = dBodyCreate (world);
dBodySetPosition (b,0,0,2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
#endif
#ifdef BALLSTACK
for (dReal z = 1; z <= 6; z+=1)
{
b = dBodyCreate (world);
dBodySetPosition (b,0,0,z*RADIUS*2);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m, 0.1);
dBodySetMass (b,&m);
sphere[spheres] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[spheres++],b);
}
#endif
#ifdef CENTIPEDE
dBodyID lastb = 0;
for (dReal y = 0; y < 10*LENGTH; y+=LENGTH+0.1)
{
// chassis body
b = body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],-15,y,STARTZ);
dMassSetBox (&m,1,WIDTH,LENGTH,HEIGHT);
dMassAdjust (&m,CMASS);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,WIDTH,LENGTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
for (dReal x = -17; x > -20; x-=RADIUS*2)
{
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *a = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,-10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
body[bodies] = dBodyCreate (world);
dBodySetPosition(body[bodies], -30 - x, y, STARTZ);
dMassSetSphere(&m, 1, RADIUS);
dMassAdjust(&m, WMASS);
dBodySetMass(body[bodies], &m);
sphere[spheres] = dCreateSphere (space, RADIUS);
dGeomSetBody (sphere[spheres++], body[bodies]);
joint[joints] = dJointCreateHinge2 (world,0);
if (x == -17)
dJointAttach (joint[joints],b,body[bodies]);
else
dJointAttach (joint[joints],body[bodies-2],body[bodies]);
const dReal *b = dBodyGetPosition (body[bodies++]);
dJointSetHinge2Anchor (joint[joints],b[0],b[1],b[2]);
dJointSetHinge2Axis1 (joint[joints],0,0,1);
dJointSetHinge2Axis2 (joint[joints],1,0,0);
dJointSetHinge2Param (joint[joints],dParamSuspensionERP,1.0);
dJointSetHinge2Param (joint[joints],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[joints],dParamLoStop,0);
dJointSetHinge2Param (joint[joints],dParamHiStop,0);
dJointSetHinge2Param (joint[joints],dParamVel2,10.0);
dJointSetHinge2Param (joint[joints++],dParamFMax2,FMAX);
}
if (lastb)
{
dJointID j = dJointCreateFixed(world,0);
dJointAttach (j, b, lastb);
dJointSetFixed(j);
}
lastb = b;
}
#endif
#ifdef BOX
body[bodies] = dBodyCreate (world);
dBodySetPosition (body[bodies],0,0,HEIGHT/2);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m, 1);
dBodySetMass (body[bodies],&m);
box[boxes] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxes++],body[bodies++]);
#endif
#ifdef CANNON
cannon_ball_body = dBodyCreate (world);
cannon_ball_geom = dCreateSphere (space,CANNON_BALL_RADIUS);
dMassSetSphereTotal (&m,CANNON_BALL_MASS,CANNON_BALL_RADIUS);
dBodySetMass (cannon_ball_body,&m);
dGeomSetBody (cannon_ball_geom,cannon_ball_body);
dBodySetPosition (cannon_ball_body,CANNON_X,CANNON_Y,CANNON_BALL_RADIUS);
#endif
}
IoObject *IoODEBody_position(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
return IoVector_newWithODEPoint(IOSTATE, dBodyGetPosition(BODYID));
}
Vector3D StickyObj::getPosition(){
const dReal *pos = dBodyGetPosition(bodyID);
return Vector3D(pos[0],pos[1],pos[2]);
}
Simulator::Simulator(const double posx, const double posy)
{
m_collision = false;
//Open Dynamics Engine stuff
m_world = dWorldCreate();
m_space = dHashSpaceCreate(0);
m_contacts = dJointGroupCreate(0);
m_ground = dCreatePlane(m_space, 0, 0, 1, 0);
dGeomSetData(m_ground, (void *) &TYPE_TERRAIN);
dWorldSetGravity(m_world, 0, 0, -0.81);
//create robot
const double LENGTH = 2.50; // chassis length 2.50;
const double WIDTH = 1.00; // chassis width
const double HEIGHT = 0.40; // chassis height
const double RADIUS = 0.45 * WIDTH;//0.45 * WIDTH; // wheel radius
const double STARTZ = 0.05;
dMass m;
dQuaternion q;
double car_center_x= posx + 1.5; //1.5
double car_center_y= posy + 5.3;
// chassis body
m_robotBodyChassis = dBodyCreate(m_world);
dBodySetPosition(m_robotBodyChassis, car_center_x, car_center_y, 0.85 * RADIUS + 0.5 * HEIGHT + STARTZ);
dMassSetBox(&m, 1, LENGTH, WIDTH, HEIGHT);
dBodySetMass(m_robotBodyChassis,&m);
// chassis geometry
m_robotGeomChassis = dCreateBox(m_space, LENGTH, WIDTH, HEIGHT);
dGeomSetBody(m_robotGeomChassis, m_robotBodyChassis);
m_robotBodies.push_back(m_robotBodyChassis);
dGeomSetData(m_robotGeomChassis, (void *) &TYPE_ROBOT);
// wheel bodies
for(int i= 0; i < 3; i++)
{
m_robotBodyWheels[i] = dBodyCreate(m_world);
dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5);
dBodySetQuaternion(m_robotBodyWheels[i], q);
dMassSetSphere(&m, 1, RADIUS);
dBodySetMass(m_robotBodyWheels[i], &m);
m_robotGeomWheels[i] = dCreateSphere(m_space, RADIUS);
dGeomSetBody(m_robotGeomWheels[i], m_robotBodyWheels[i]);
m_robotBodies.push_back(m_robotBodyWheels[i]);
dGeomSetData(m_robotGeomWheels[i], (void *) &TYPE_ROBOT);
}
dBodySetPosition(m_robotBodyWheels[0], car_center_x + 0.5 * LENGTH, car_center_y, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[1], car_center_x - 0.5 * LENGTH, car_center_y + 0.5 * WIDTH, RADIUS + STARTZ);
dBodySetPosition(m_robotBodyWheels[2], car_center_x - 0.5 * LENGTH, car_center_y - 0.5 * WIDTH, RADIUS + STARTZ);
// front and back wheel hinges
for (int i = 0; i < 3; i++)
{
m_robotJoints[i] = dJointCreateHinge2(m_world, 0); // creat hign-2 joint as wheels
dJointAttach(m_robotJoints[i], m_robotBodyChassis, m_robotBodyWheels[i]);
const dReal *a = dBodyGetPosition(m_robotBodyWheels[i]);
dJointSetHinge2Anchor(m_robotJoints[i], a[0], a[1], a[2]);
dJointSetHinge2Axis1(m_robotJoints[i], 0, 0, 1);
dJointSetHinge2Axis2(m_robotJoints[i], 0, 1, 0);
// set joint suspension
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionERP, 0.04);
dJointSetHinge2Param(m_robotJoints[i], dParamSuspensionCFM, 0.08);
}
// lock back wheels along the steering axis
for (int i = 1; i < 3; i++)
{
// set stops to make sure wheels always stay in alignment
dJointSetHinge2Param (m_robotJoints[i], dParamLoStop, 0);
dJointSetHinge2Param (m_robotJoints[i], dParamHiStop, 0);
}
}
// 制御式
static void control() {
mytime += 0.0001;
/* // ロッドのマニュアル制御
dReal rod_q = dJointGetHingeAngle(rod_joint[1]);
dReal rod_input = 5.0 * (rod_q_d - rod_q);
dJointSetHingeParam(rod_joint[1], dParamVel, rod_input);
dJointSetHingeParam(rod_joint[1], dParamFMax, 10000.0);
*/
// 弾丸の位置を取得
const dReal *pos, *vel; // 弾丸の位置と速度ベクトル
dReal tar[3] = {0.0}; // 計算後の着弾予想点
dReal rot[12] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0};
//const dReal *tar_pos, *tar_rot; // 着弾予想地点
dReal q_p_d = 0.0; // 広域レーダーからの目標角度
dReal q_c_d = 0.0; // 近接センサからの偏差
dReal distance = 0.0; // SHIELD中心から弾丸までの距離
dReal rod_q = dJointGetHingeAngle(rod_joint[1]); // RODの現在角度
dReal rod_input = 0.0; // ROD制御入力
pos = dBodyGetPosition(bullet.body);
vel = dBodyGetLinearVel(bullet.body);
//pos = dBodyGetRelPointPos(bullet.body);
//vel = dBodyGetRelPointVel(bullet.body);
distance = sqrt(
(SHIELD_X-pos[0])*(SHIELD_X-pos[0]) +
(SHIELD_Y-pos[1])*(SHIELD_Y-pos[1]) +
(SHIELD_Z-pos[2])*(SHIELD_Z-pos[2])
);
if(abs(vel[1])>0.1){
tar[0] = pos[0] + pos[1]*vel[0]/abs(vel[1]);
tar[1] = 0.0;
tar[2] = pos[2] + pos[1]*vel[2]/abs(vel[1]);
// 着弾予想点の描画
dsSetColor(1 ,0 ,0);
dsDrawSphere(tar, rot, BULLET_RADIUS+0.1);
// 広域レーダー
if(distance < 50.0){
q_p_d = 1.0 * atan(tar[0]/(tar[2]-SHIELD_Z));
}
// 近接センサ
if(distance < 10.0){
q_c_d = -1.0 * atan(pos[0]/pos[2]);
diff_sum += q_c_d - rod_q;
}
}
// 入力トルク（疑似付加目標角度）を計算
if(act_flg){ // スイッチがオンの場合
rod_input = 500.0 * (q_p_d - rod_q); // 通常制御のみ
//rod_input = 50.0 * (q_p_d - rod_q) + 0.001 * diff_sum;
rod_q_d = rod_q;
//FILE *fp_0; fp_0 = fopen("dataA.csv","a");
//fprintf(fp_0,"%lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf\n", mytime, pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], tar[0], tar[1], tar[2], q_p_d, q_c_d, rod_q);
//fclose(fp_0);
}else{
rod_input = 500.0 * (rod_q_d - rod_q); // マニュアル操作
}
//rod_input = 500.0 * (rod_q_d - rod_q); // マニュアル操作
printf("%lf : %lf -> %lf = %lf\r", mytime, rod_q_d, rod_q, rod_input);
//printf("%lf, %lf, %lf\r", pos[0], pos[1], pos[2] );
//printf("%lf, %lf, %lf\r", vel[0], vel[1], vel[2] );
//printf("%lf, %lf, %lf\r", rod_q_d*180.0/M_PI, rod_q*180.0/M_PI, rod_input);
// 制御入力
dJointSetHingeParam(rod_joint[1], dParamVel, rod_input);
dJointSetHingeParam(rod_joint[1], dParamFMax, 100000.0);
}
const Fvector& CPHActivationShape:: Position ()
{
return cast_fv(dBodyGetPosition(m_body));
}
Car::Car(dWorldID world, dSpaceID space, CarDesignID n_car_design, AppDesignID n_app_design) :MyObject(world, space), car_design(n_car_design),
app_design(n_app_design)
{
int i, j, ind;
body_obj = 0;
for (i = 0; i < 2; ++i)
chain_obj[i] = 0;
for (i = 0; i < 6; ++i)
wheel_obj[i] = 0;
max_f = 0; // must be set with setMaxF
TrackDesignID track_design = car_design->left_track_design;
LinkDesignID link_design = track_design->link_design;
WheelDesignID wheel_design = car_design->wheel_design;
body_mass = car_design->getBodyMass();
// create body
createBody(car_design->getChassisPosition());
/*
for ( int i = 0 ; i < 6 ; ++i ) {
CreateToothedWheel(world, space,
wheel_obj[i], sprocket_teeth, R, t_sides, teeth_h_fuzz);
}
*/
// create sprocket wheels in back wheels position
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
sprocket[j] = wheel_obj[ind]->body[0];
const dReal *sprocket_pos =
track_design->getBackWheelPos();
PVEC(sprocket_pos);
dReal y = sprocket_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(sprocket[j], sprocket_pos[XX], y,
sprocket_pos[ZZ]);
}
// create sprockets and set their position
// create front wheels and set their position
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
wheel_obj[ind] = wheel_design->create(world, space);
front[j] = wheel_obj[ind]->body[0];
const dReal *front_pos = track_design->getFrontWheelPos();
PVEC(front_pos);
dReal y = front_pos[YY];
if (j == 1)
y += car_design->getTrackToTrack();
PEXP(y);
dBodySetPosition(front[j], front_pos[XX], y,
front_pos[ZZ]);
}
// create chains
chain_obj[0] =
new Chain(world, space, track_design, link_design);
track_design->moveDesign(0, car_design->getTrackToTrack(), 0); // instead of recreating design
chain_obj[1] = new Chain(world, space, track_design, link_design);
// create axle joints for sprockets
for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], sprocket[j], second);
const dReal *sp_v = dBodyGetPosition(sprocket[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], sp_v[XX], sp_v[YY],
sp_v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
}
/*
for ( j = 0, ind = FIRST_BACK ; j < 2 ; ++j, ++ind ) {
axle[ind] = dJointCreateHinge(world, 0); // no joint group (0==NULL)
dJointAttach(axle[ind], back[j], 0); // attach to world
const dReal* v = dBodyGetPosition(back[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], y_axis[XX], y_axis[YY], y_axis[ZZ]); // axis is up (positive z)
}
*/
// create front wheels joints
for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
axle[ind] = dJointCreateHinge(world, 0);
dBodyID second = (body_obj ? body_obj->body[0] : 0);
dJointAttach(axle[ind], front[j], second);
const dReal *v = dBodyGetPosition(front[j]);
const dReal *rot = dBodyGetRotation(sprocket[j]);
dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
} if (body_obj)
body_obj->show_forces = 1;
// create appendage
if (body_obj && app_design) {
//app_obj = new App(world, space, app_design, body_obj->body[0]);
app_obj = 0;
};
// lets see what we have, height wise
if (chain_obj[0] != 0) {
const dReal *pos =
dBodyGetPosition(chain_obj[0]->body[0]);
std::cout << "debug: first link position, z wise\n";
PEXP(pos[ZZ]);
}
#ifdef ARE_YOU_NUTS_ABOUT_SPRINGS
// now create the suspension - it is between the wheels and the body.
Init(0, 2, 0);
// initially lets make some springs between the wheels and the body.
for (int i = 0 ; i < 2 ; ++i) {
dBody a_wheel = wheel_obj[i + FIRST_SPROCKET]->body[0];
};
#endif
}
dReal doStuffAndGetError (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the orientations are the same
const dReal *R1 = dBodyGetRotation (body[0]);
const dReal *R2 = dBodyGetRotation (body[1]);
dReal err1 = dMaxDifference (R1,R2,3,3);
// check the body offset is correct
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return (err1 + length (pp)) * 300;
}
case 1: { // 1 body to static env
addOscillatingTorque (0.1);
// check the orientation is the identity
dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return (err1 + length (p)) * 1e6;
}
case 2: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the body offset is correct
// Should really check body rotation too. Oh well.
const dReal *R1 = dBodyGetRotation (body[0]);
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return length(pp) * 300;
}
case 3: { // 1 body to static env with relative rotation
addOscillatingTorque (0.1);
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return length (p) * 1e6;
}
// ********** hinge joint
case 200: // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
return dInfinity;
case 220: // hinge angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHingeAngle (joint);
if (a > 0.5 && a < 1) return 0; else return 10;
}
return 0;
case 221: { // hinge angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHingeAngle (joint);
dReal r = dJointGetHingeAngleRate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er) * 4e4;
}
case 230: // hinge motor rate (and polarity) test
case 231: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHingeAngleRate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHingeParam (joint,dParamVel,cos(a));
if (n==231) return dInfinity;
return err * 1e6;
}
// ********** slider joint
case 300: // 2 body
addOscillatingTorque (0.05);
dampRotationalMotion (0.1);
addSpringForce (0.5);
return dInfinity;
case 320: // slider angle polarity test
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
if (iteration == 40) {
dReal a = dJointGetSliderPosition (joint);
if (a > 0.2 && a < 0.5) return 0; else return 10;
return a;
}
return 0;
case 321: { // slider angle rate test
static dReal last_pos = 0;
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
dReal p = dJointGetSliderPosition (joint);
dReal r = dJointGetSliderPositionRate (joint);
dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
last_pos = p;
return fabs(r-er) * 1e9;
}
case 330: // slider motor rate (and polarity) test
case 331: { // ...with stops
static dReal a = 0;
dReal r = dJointGetSliderPositionRate (joint);
dReal err = fabs (0.7*cos(a) - r);
if (a < 0.04) err = 0;
a += 0.03;
dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
if (n==331) return dInfinity;
return err * 1e6;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHinge2Angle1 (joint);
if (a > 0.5 && a < 0.6) return 0; else return 10;
}
return 0;
case 421: { // hinge-2 steering angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHinge2Angle1 (joint);
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er)*2e4;
}
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel,cos(a));
if (n==431) return dInfinity;
return err * 1e6;
}
case 432: { // hinge 2 wheel motor rate (+polarity) test
static dReal a = 0;
dReal r = dJointGetHinge2Angle2Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel2,cos(a));
return err * 1e6;
}
// ********** angular motor joint
case 600: { // test euler angle calculations
// desired euler angles from last iteration
static dReal a1,a2,a3;
// find actual euler angles
dReal aa1 = dJointGetAMotorAngle (joint,0);
dReal aa2 = dJointGetAMotorAngle (joint,1);
dReal aa3 = dJointGetAMotorAngle (joint,2);
// printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
dReal err = dInfinity;
if (iteration > 0) {
err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
err *= 1e10;
}
// get random base rotation for both bodies
dMatrix3 Rbase;
dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
dBodySetRotation (body[0],Rbase);
// rotate body 2 by random euler angles w.r.t. body 1
a1 = 3.14 * 2 * (dRandReal()-0.5);
a2 = 1.57 * 2 * (dRandReal()-0.5);
a3 = 3.14 * 2 * (dRandReal()-0.5);
dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
dRFromAxisAndAngle (R1,0,0,1,-a1);
dRFromAxisAndAngle (R2,0,1,0,a2);
dRFromAxisAndAngle (R3,1,0,0,-a3);
dMultiply0 (Rtmp1,R2,R3,3,3,3);
dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
dBodySetRotation (body[1],Rtmp1);
// printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
return err;
}
// ********** universal joint
case 700: { // 2 body: joint constraint
dVector3 ax1, ax2;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 701: { // 2 body: angle 1 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 702: { // 2 body: angle 2 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 720: { // universal transmit torque test: constraint error
dVector3 ax1, ax2;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 721: { // universal transmit torque test: angle1 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 722: { // universal transmit torque test: angle2 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 730:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 731:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 2e3;
}
case 732:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 740:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 741:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 742:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e4;
}
}
return dInfinity;
}
bool CPHActivationShape:: Activate (const Fvector need_size,u16 steps,float max_displacement,float max_rotation,bool un_freeze_later/* =false*/)
{
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgDrawDeathActivationBox))
{
DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;dGeomBoxGetLengths(m_geom,cast_fp(v));v.mul(0.5f);
DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,0));
}
#endif
VERIFY(m_geom&&m_body);
CPHObject::activate();
ph_world->Freeze();
UnFreeze();
max_depth=0.f;
dGeomUserDataSetObjectContactCallback(m_geom,GetMaxDepthCallback) ;
//ph_world->Step();
ph_world->StepTouch();
u16 num_it =15;
float fnum_it=float(num_it);
float fnum_steps=float(steps);
float fnum_steps_r=1.f/fnum_steps;
float resolve_depth=0.01f;
float max_vel=max_depth/fnum_it*fnum_steps_r/fixed_step;
float limit_l_vel=_max(_max(need_size.x,need_size.y),need_size.z)/fnum_it*fnum_steps_r/fixed_step;
if(limit_l_vel>default_l_limit)
limit_l_vel=default_l_limit;
if(max_vel>limit_l_vel)
max_vel=limit_l_vel;
float max_a_vel=max_rotation/fnum_it*fnum_steps_r/fixed_step;
if(max_a_vel>default_w_limit)
max_a_vel=default_w_limit;
//ph_world->CutVelocity(0.f,0.f);
dGeomUserDataSetCallbackData(m_geom,this);
dGeomUserDataSetObjectContactCallback( m_geom, ActivateTestDepthCallback );
if( m_flags.test( flStaticEnvironment ) )
dGeomUserDataAddObjectContactCallback(m_geom,StaticEnvironment);
max_depth=0.f;
Fvector from_size;
Fvector step_size,size;
dGeomBoxGetLengths(m_geom,cast_fp(from_size));
step_size.sub(need_size,from_size);
step_size.mul(fnum_steps_r);
size.set(from_size);
bool ret=false;
V_PH_WORLD_STATE temp_state;
ph_world->GetState(temp_state);
for(int m=0;steps>m;++m)
{
//float param =fnum_steps_r*(1+m);
//InterpolateBox(id,param);
size.add(step_size);
dGeomBoxSetLengths(m_geom,size.x,size.y,size.z);
u16 attempts=10;
do{
ret=false;
for(int i=0;num_it>i;++i)
{
max_depth=0.f;
ph_world->Step();
CHECK_POS(Position(),"pos after ph_world->Step()",false);
ph_world->CutVelocity(max_vel,max_a_vel);
CHECK_POS(Position(),"pos after CutVelocity",true);
//if(m==0&&i==0)ph_world->GetState(temp_state);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
attempts--;
}while(!ret&&attempts>0);
#ifdef DEBUG
Msg("correction attempts %d",10-attempts);
#endif
}
RestoreVelocityState(temp_state);
CHECK_POS(Position(),"pos after RestoreVelocityState(temp_state);",true);
if(!un_freeze_later)ph_world->UnFreeze();
#ifdef DEBUG
if(ph_dbg_draw_mask.test(phDbgDrawDeathActivationBox))
{
DBG_OpenCashedDraw();
Fmatrix M;
PHDynamicData::DMXPStoFMX(dBodyGetRotation(m_body),dBodyGetPosition(m_body),M);
Fvector v;v.set(need_size);v.mul(0.5f);
DBG_DrawOBB(M,v,D3DCOLOR_XRGB(0,255,255));
DBG_ClosedCashedDraw(30000);
}
#endif
return ret;
}
bool CPHMovementControl:: ActivateBoxDynamic(DWORD id,int num_it/*=8*/,int num_steps/*5*/,float resolve_depth/*=0.01f*/)
{
bool character_exist=CharacterExist();
if(character_exist&&trying_times[id]!=u32(-1))
{
Fvector dif;dif.sub(trying_poses[id],cast_fv(dBodyGetPosition(m_character->get_body())));
if(Device.dwTimeGlobal-trying_times[id]<500&&dif.magnitude()<0.05f)
return false;
}
if(!m_character||m_character->PhysicsRefObject()->PPhysicsShell())return false;
DWORD old_id=BoxID();
bool character_disabled=character_exist && !m_character->IsEnabled();
if(character_exist&&id==old_id)return true;
if(!character_exist)
{
CreateCharacter();
}
//m_PhysicMovementControl->ActivateBox(id);
m_character->CPHObject::activate();
ph_world->Freeze();
UnFreeze();
saved_callback=ObjectContactCallback();
SetOjectContactCallback(TestDepthCallback);
SetFootCallBack(TestFootDepthCallback);
max_depth=0.f;
//////////////////////////////////pars///////////////////////////////////////////
// int num_it=8;
// int num_steps=5;
// float resolve_depth=0.01f;
if(!character_exist)
{
num_it=20;
num_steps=1;
resolve_depth=0.1f;
}
///////////////////////////////////////////////////////////////////////
float fnum_it=float(num_it);
float fnum_steps=float(num_steps);
float fnum_steps_r=1.f/fnum_steps;
Fvector vel;
Fvector pos;
GetCharacterVelocity(vel);
GetCharacterPosition(pos);
//const Fbox& box =Box();
float pass= character_exist ? _abs(Box().getradius()-boxes[id].getradius()) : boxes[id].getradius();
float max_vel=pass/2.f/fnum_it/fnum_steps/fixed_step;
float max_a_vel=M_PI/8.f/fnum_it/fnum_steps/fixed_step;
dBodySetForce(GetBody(),0.f,0.f,0.f);
dBodySetLinearVel(GetBody(),0.f,0.f,0.f);
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
CVelocityLimiter vl(GetBody(),max_vel,max_vel);
max_vel=1.f/fnum_it/fnum_steps/fixed_step;
bool ret=false;
m_character->SwitchOFFInitContact();
vl.Activate();
vl.l_limit*=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
////////////////////////////////////
for(int m=0;30>m;++m)
{
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
max_depth=0.f;
ph_world->Step();
if(max_depth < resolve_depth)
{
break;
}
ph_world->CutVelocity(max_vel,max_a_vel);
}
vl.l_limit/=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
/////////////////////////////////////
for(int m=0;num_steps>m;++m)
{
float param =fnum_steps_r*(1+m);
InterpolateBox(id,param);
ret=false;
for(int i=0;num_it>i;++i){
max_depth=0.f;
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
ph_world->Step();
ph_world->CutVelocity(max_vel,max_a_vel);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
if(!ret) break;
}
m_character->SwitchInInitContact();
vl.Deactivate();
ph_world->UnFreeze();
if(!ret)
{
if(!character_exist)DestroyCharacter();
else if(character_disabled)m_character->Disable();
ActivateBox(old_id);
SetVelocity(vel);
dBodyID b=GetBody();
if(b)
{
dMatrix3 R;
dRSetIdentity (R);
dBodySetAngularVel(b,0.f,0.f,0.f);
dBodySetRotation(b,R);
}
SetPosition(pos);
//Msg("can not activate!");
}
else
{
ActivateBox(id);
//Msg("activate!");
}
SetOjectContactCallback(saved_callback);
SetVelocity(vel);
saved_callback=0;
if(!ret&&character_exist)
{
trying_times[id]=Device.dwTimeGlobal;
trying_poses[id].set(cast_fv(dBodyGetPosition(m_character->get_body())));
}
else
{
trying_times[id]=u32(-1);
}
return ret;
}
void reset_test()
{
int i;
dMass m,anchor_m;
dReal q[NUM][3], pm[NUM]; // particle positions and masses
dReal pos1[3] = {1,0,1}; // point of reference (POR)
dReal pos2[3] = {-1,0,1}; // point of reference (POR)
// make random particle positions (relative to POR) and masses
for (i=0; i<NUM; i++) {
pm[i] = dRandReal()+0.1;
q[i][0] = dRandReal()-0.5;
q[i][1] = dRandReal()-0.5;
q[i][2] = dRandReal()-0.5;
}
// adjust particle positions so centor of mass = POR
computeMassParams (&m,q,pm);
for (i=0; i<NUM; i++) {
q[i][0] -= m.c[0];
q[i][1] -= m.c[1];
q[i][2] -= m.c[2];
}
if (world) dWorldDestroy (world);
world = dWorldCreate();
anchor_body = dBodyCreate (world);
dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
dMassAdjust (&anchor_m,0.1);
dBodySetMass (anchor_body,&anchor_m);
for (i=0; i<NUM; i++) {
particle[i] = dBodyCreate (world);
dBodySetPosition (particle[i],
pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,pm[i]);
dBodySetMass (particle[i],&m);
}
for (i=0; i < NUM; i++) {
particle_joint[i] = dJointCreateBall (world,0);
dJointAttach (particle_joint[i],anchor_body,particle[i]);
const dReal *p = dBodyGetPosition (particle[i]);
dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
}
// make test_body with the same mass and inertia of the anchor_body plus
// all the particles
test_body = dBodyCreate (world);
dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
computeMassParams (&m,q,pm);
m.mass += anchor_m.mass;
for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
dBodySetMass (test_body,&m);
// rotate the test and anchor bodies by a random amount
dQuaternion qrot;
for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
dNormalize4 (qrot);
dBodySetQuaternion (anchor_body,qrot);
dBodySetQuaternion (test_body,qrot);
dMatrix3 R;
dQtoR (qrot,R);
for (i=0; i<NUM; i++) {
dVector3 v;
dMultiply0 (v,R,&q[i][0],3,3,1);
dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
}
// set random torque
for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
iteration=0;
}
int main(void)
{
chdir(getenv("HOME"));
std::srand(std::time(NULL));
// In JavaScript, this would be "var window;"
GLFWwindow* window; // This creates a variable to store the GLFW window
glfwSetErrorCallback(error_callback); // Gives GLFW a function to call when there's an error.
if (!glfwInit()) // Allows GLFW to do some initial setup and initialization.
exit(EXIT_FAILURE); // If initialization fails, we can't continue with the program.
// ODE initialization
dInitODE();
gODEWorld = dWorldCreate();
gODESpace = dHashSpaceCreate(0);
dWorldSetGravity(gODEWorld, 0, 0, -0.98);
dWorldSetCFM(gODEWorld, 1e-5);
dCreatePlane(gODESpace, 0, 0, 1, 0); // create the base plane
gODEContactGroup = dJointGroupCreate (0);
static dBodyID playBody = dBodyCreate (gODEWorld);
static dGeomID playGeom = dCreateSphere (gODESpace, 4);
dGeomSetBody (playGeom, playBody);
dMass* mass2;
mass2 = new dMass;
dMassSetBox(mass2, 1, 1, 1, 1);
dBodySetMass(playBody, mass2);
static dBodyID sphereBody = dBodyCreate (gODEWorld);
static dGeomID sphereGeom = dCreateSphere(gODESpace, 1);
dGeomSetBody (sphereGeom, sphereBody);
dMass mass3;
dMassSetSphere(&mass3, 2, 1);
dBodySetMass(sphereBody, &mass3);
// Builds a new GLFW window and saves the result in the variable above.
// If there's an error here, window will be set to 0.
// 640x480 is the initial size, and "Simple example" is the name of the window.
window = glfwCreateWindow(640, 480, "Simple example", NULL, NULL);
// If window == 0, this will be true, and we've hit an error.
if (!window /*same as saying `window == 0`*/)
{
glfwTerminate(); // This is the opposite of glfwInit, and allows GLFW to close up shop.
exit(EXIT_FAILURE); // This kills the application.
}
glfwMakeContextCurrent(window); // Tells GLFW which window is going to be drawn to.
glfwSwapInterval(1); // Tells GLFW how often the window should be redrawn.
// key_callback is the function that GLFW should call when the user hits
// a key on the keyboard. So we give that function to GLFW with this routine.
glfwSetKeyCallback(window, key_callback);
glfwSetCursorPosCallback(window, cursor_pos_callback);
glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
// Set some OpenGL gODEWorld options.
glEnable(GL_CULL_FACE);
glEnable(GL_DEPTH_TEST);
glCullFace(GL_BACK);
//comment this out to go to normal colors
glEnable(GL_TEXTURE_2D);
gTextureSteel.load();
gTexture.load();
gTextureWood.load();
gTextureLeaves.load();
gTextureRoad.load();
gTextureRoadY.load();
gTextureWhite.load();
gTextureBall.load();
gTextureClear.load();
gTextureCerealbox.load();
gTextureCerealboxNutFacts.load();
gTextureCerealboxBlank.load();
cubeD_D myCube;
myCube.setTexture(gTextureBall);
myCube.SetLocation(-10, 0, 20);
myCube.density=0.5;
myCube.SetSize(0.5, 0.5, 0.5);
cubeD_D myCube1;
myCube1.setTexture(gTextureWhite);
myCube1.SetLocation(5, 1, 49);
myCube1.density=6;
myCube1.SetSize(2, 2, 2);
BGcubeD_D BGCube1;
BGCube1.BGsetTexture(gTextureRoad);
BGCube1.BGSetLocation(4, 5, 7);
BGCube1.BGSetSize(20, 1, 14);
BGcubeD_D BGCube2;
BGCube2.BGsetTexture(gTextureRoad);
BGCube2.BGSetLocation(14.5, 14.5, 7);
BGCube2.BGSetSize(1, 20, 14);
BGcubeD_D BGCube3;
BGCube3.BGsetTexture(gTextureRoad);
BGCube3.BGSetLocation(-6.5, 14.5, 7);
BGCube3.BGSetSize(1, 20, 14);
BGcubeD_D BGCube4;
BGCube4.BGsetTexture(gTextureRoad);
BGCube4.BGSetLocation(4, 24, 7);
BGCube4.BGSetSize(20, 1, 14);
BGcubeD_D Road1;
Road1.BGsetTexture(gTextureRoad);
Road1.BGSetLocation(0, 0, 0.1);
Road1.BGSetSize(1000, 8, 0.5);
cubeD_D myCube3;
myCube3.setTexture(gTextureRoadY);
myCube3.SetLocation(4, 0, 89);
myCube3.density=2;
myCube1.SetSize(4, 4, 4);
cubeD_D BouncyBlock;
BouncyBlock.setTexture(gTextureSteel);
BouncyBlock.SetLocation(0, 3, 40);
BouncyBlock.r_m=255;
BouncyBlock.g_m=0;
BouncyBlock.b_m=0;
dBodyAddForce(BouncyBlock.boxBody_m, 5, 5, 0);
myCube1.SetSize(7, 7, 7);
cubeD_D cerealbox;
cerealbox.tex4=gTextureCerealbox;
cerealbox.tex3=gTextureCerealboxNutFacts;
cerealbox.tex2=gTextureCerealboxNutFacts;
cerealbox.tex1=gTextureCerealboxBlank;
cerealbox.tex5=gTextureCerealboxBlank;
cerealbox.tex6=gTextureCerealboxBlank;
cerealbox.SetLocation(4, -20, 1);
cerealbox.density=1;
cerealbox.SetSize(2, 1, 2.5);
cubeD_D freezerBack;
freezerBack.setTexture(gTextureWhite);
freezerBack.SetLocation(20, 30, 2.5);
freezerBack.SetSize(10,1,5);
dBodySetPosition(sphereBody,0,0,50);
static const float simulation_start_k = glfwGetTime();
static const float real_min_per_game_day_k = 24; // CHANGE ONLY HERE TO AFFECT DAY/NIGHT SPEED
static const float real_sec_per_game_day_k = real_min_per_game_day_k * 60;
static const float real_sec_per_game_hrs_k = real_sec_per_game_day_k / 24;
static const float real_sec_per_game_min_k = real_sec_per_game_hrs_k / 60;
static const float game_min_per_real_sec_k = 1 / real_sec_per_game_min_k;
static const float min_per_day_k = 24 * 60;
//look position
camRotateX=-90;
// This is the main processing loop that draws the spinning rectangle.
while (!glfwWindowShouldClose(window)) // this will loop until the window should close.
{
float elapsed_real_sec = glfwGetTime() - simulation_start_k;
float elapsed_game_min = game_min_per_real_sec_k * elapsed_real_sec;
float elapsed_game_hrs = elapsed_game_min / 60;
float percent_of_day = (static_cast<int>(elapsed_game_min) % static_cast<int>(min_per_day_k)) / min_per_day_k;
float sky_cycle = std::sin(percent_of_day * M_PI);
float sky = 0 * (1-sky_cycle) + 0.9803921569 * sky_cycle;
// int game_hrs_mil = static_cast<int>(elapsed_game_hrs) % 24; // military hours
// Set to #if 1 to enable displaying the time
#if 0
std::cout.width(2);
std::cout.fill('0');
std::cout << game_hrs_mil << ":";
std::cout.width(2);
std::cout << (static_cast<int>(elapsed_game_min)%60) << '\n';
#endif
// Simulate the physics engine
// find collisions and add contact joints
dSpaceCollide (gODESpace, 0, &ODEContactCallback);
// step the simulation
dWorldQuickStep (gODEWorld, 0.1);
// remove all contact joints
dJointGroupEmpty (gODEContactGroup);
// These are variable declarations.
int width, height; // these variables store the dimensions of the window
glfwGetFramebufferSize(window, &width, &height); // Get the height and width of the window from GLFW.
float ratio = width / (float) height; // compute the aspect ratio of the window, which we need below.
glViewport(0, 0, width, height); // This tells OpenGL how big the window is,
glClearColor(0.5294117648+sky-0.9803921569, 0.8078431373+sky-0.9803921569, sky, 0); // and OpenGL goes off and creates a space
// for drawing.
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // This asks OpenGL to wipe clean the drawing space.
// The default color is black. If you want it to be
// another color, you have to call glClearColor with
// the new color values before making this call.
/*
These operations tell OpenGL how to convert the 3D world we are about
to create into a 2D image that can be displayed on the computer screen.
*/
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, ratio, 1, 1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(camRotateX, 1.f, 0.f, 0.f);
glRotatef(camRotateY, 0.f, 0.f, 1.f);
const dReal* pos = dBodyGetPosition(playBody);
if(MoveForward){
//dBodySetPosition (playBody, camX-2,camY+2.5,camZ);
//camX -= std::sin(DegreesToRads(camRotateY))*0.1;
//camY -= std::cos(DegreesToRads(camRotateY))*0.1;
dBodySetForce(playBody, std::sin(DegreesToRads(camRotateY))*1, std::cos(DegreesToRads(camRotateY))*1, 0);
}
if(MoveRight){
//camY += std::cos(DegreesToRads(camRotateY-90))*0.1;
//camX += std::sin(DegreesToRads(camRotateY-90))*0.1;
dBodySetForce(playBody, -std::sin(DegreesToRads(camRotateY-90))*1, -std::cos(DegreesToRads(camRotateY-90))*1, 0);
}
if(MoveLeft){
//camY += std::cos(DegreesToRads(camRotateY+90))*0.1;
//camX += std::sin(DegreesToRads(camRotateY+90))*0.1;
dBodySetForce(playBody, -std::sin(DegreesToRads(camRotateY+90))*1, -std::cos(DegreesToRads(camRotateY+90))*1, 0);
}
if(MoveBackward){
//camY += std::cos(DegreesToRads(camRotateY))*0.1;
//camX += std::sin(DegreesToRads(camRotateY))*0.1;
dBodySetForce(playBody, -std::sin(DegreesToRads(camRotateY))*1, -std::cos(DegreesToRads(camRotateY))*1, 0);
}
if(MoveUp){
//camZ += DecreaseClimbRate;
//DecreaseClimbRate-=0.0077;
camZ=pos[2]-1;
dBodySetForce(playBody, 0,0,2);
}
if(MoveDown){
//camZ += DecreaseClimbRate;
//DecreaseClimbRate-=0.0077;
camZ=pos[2]-1;
dBodySetForce(playBody, 0,0,-2);
}
if(Sprint && CarSprint){
camY += std::cos(DegreesToRads(camRotateY))*-0.55;
camX += std::sin(DegreesToRads(camRotateY))*-0.55;
}
if(CarSprint){
camY -= std::cos(DegreesToRads(camRotateY))*1;
camX -= std::sin(DegreesToRads(camRotateY))*1;
}
if(Zoom){
camY -= std::cos(DegreesToRads(camRotateY))*50.5;
camX -= std::sin(DegreesToRads(camRotateY))*50.5;
}
if(MoveUp==true){
camZ=pos[2]-1;
}
if(MoveUp==false && MoveDown==false){
camZ=pos[2]-1;
}
const dReal* speedSAVE = dBodyGetLinearVel(playBody);
//std::cout << "X=" << speedSAVE[1] << '\n';
if(MoveForward==false && MoveBackward==false && MoveLeft==false && MoveRight==false){
//dBodyGetLinearVel(cubeD_D().boxBody_m);
//speedy = -std::cos(DegreesToRads(camRotateY))*1
if(speedSAVE[0]>0.2){
dBodySetForce(playBody, -2.5, 0, 0);
}
if(speedSAVE[0]<-0.2){
dBodySetForce(playBody, 2.5 , 0, 0);
}
if(speedSAVE[1]>0.2){
dBodySetForce(playBody, 0, -2.5, 0);
}
if(speedSAVE[1]<-0.2){
dBodySetForce(playBody, 0, 2.5, 0);
}
//dBodySetPosition (playBody, camX,camY,camZ);
}
/*if(MoveLeft==false){
//dBodyGetLinearVel(cubeD_D().boxBody_m);
//speedy = -std::cos(DegreesToRads(camRotateY))*4
if(speedSAVE[1]<0.2){
dBodySetForce(playBody, 0, -std::cos(DegreesToRads(camRotateY))*4, 0);
}
if(speedSAVE[1]>-0.2){
dBodySetForce(playBody, 0, -std::cos(DegreesToRads(camRotateY))*4, 0);
}
}*/
//dBodySetPosition (playBody, -camX-2,-camY+2.5,camZ+1);
if(camZ<=0){
camZ += 0.1;
DecreaseClimbRate=0.2;
MoveUp=false;
}
if(camX>=1){
fall=true;
}
else{fall=false;}
if(MouseOut==true){
glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_NORMAL);
}
if(MouseOut==false){
glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);
}
if(MouseOut==true){
glfwSetCursorPosCallback(window, 0);
}
if(MouseOut==false){
glfwSetCursorPosCallback(window, cursor_pos_callback);
}
glTranslatef(camX+2, camY-2.5, -camZ-2);
gTexture.activate();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
glBegin(GL_QUADS); // All OpenGL drawing begins with a glBegin.
glColor3f(1, 1, 1);
glTexCoord2f(0, 0); glVertex3f(-450, -450, 0);
glTexCoord2f(450, 0); glVertex3f(450, -450 ,0);
glTexCoord2f(450, 450); glVertex3f(450, 450, 0);
glTexCoord2f(0, 450); glVertex3f(-450, 450, 0);
glEnd(); // All OpenGL drawing ends with a glEnd.
//If you would like to make a custom make change this to true v
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
camX=-pos[0]-2;
camY=-pos[1]+2.5;
/*glPushMatrix();
orient_body_in_opengl(playBody);
gTextureBall.activate();
GLUquadricObj*quad2=gluNewQuadric();
gluQuadricTexture( quad2, GL_TRUE);
gluSphere(quad2, 0.5, 15, 15);
gluDeleteQuadric(quad2);
glPopMatrix();*/
// Position and draw the sphere
glPushMatrix();
orient_body_in_opengl(sphereBody);
gTextureBall.activate();
GLUquadricObj*quad=gluNewQuadric();
gluQuadricTexture( quad, GL_TRUE);
gluSphere(quad, 0.5, 15, 15);
gluDeleteQuadric(quad);
glPopMatrix();
myCube.draw();
myCube1.draw();
BGCube1.BGdraw();
BGCube2.BGdraw();
BGCube3.BGdraw();
BGCube4.BGdraw();
//dBodyDisable(myCube2.boxBody_m);
myCube3.draw();
BouncyBlock.draw();
Road1.BGdraw();
cerealbox.draw();
freezerBack.draw();
/*const dReal* pos = dBodyGetPosition(BouncyBlock.boxBody_m);
camX=-pos[0]-2;
camY=-pos[1]+2.5;
camZ=pos[2];*/
//dBodySetPosition (playBody, camX-2,camY+2.5,camZ);
camX=-pos[0]-2;
camY=-pos[1]+2.5;
dBodySetPosition (playBody, pos[0],pos[1],camZ+1);
//std::cout << "X=" << camX << '\n';
//std::cout << "Y=" << camY << '\n';
//std::cout << "Z=" << camZ << '\n';
//dMatrix3* R;
//dBodySetRotation(playBody, *R);
//dBodySetForce(playBody, 0, 0, 0);
// SwapBuffers causes the background drawing to get slapped onto the
// display for the user to see.
glfwSwapBuffers(window);
// This lets GLFW monitor event queues like keyboard and mouse events.
// It's at this time GLFW will call your callbacks to let you handle
// the events any way you would like.
glfwPollEvents();
} // end of the while loop - do it all again!
// At this point the window should be destroyed. This is the opposite routine
// for glfwCreateWindow.
glfwDestroyWindow(window);
// ODE teardown
dJointGroupDestroy (gODEContactGroup);
dSpaceDestroy (gODESpace);
dWorldDestroy (gODEWorld);
dCloseODE();
// This is the opposite of glfwInit - do some final cleanup before quitting.
glfwTerminate();
// Quit the program.
exit(EXIT_SUCCESS);
}
WorldPhysics::WorldPhysics() {
enable_complex=0;
bulldozer_state=1;
tmp_scalar=0;
tmp_wait=0;
qsrand(QTime::currentTime().msec());
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.81);
//ground_cheat = dCreatePlane (space,0,0,1,0);
wall1=dCreatePlane (space,-1,0,0,-100);
wall2=dCreatePlane (space,1,0,0,0);
wall3=dCreatePlane (space,0,-1,0,-100);
wall4=dCreatePlane (space,0,1,0,0);
// our heightfield floor
dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
// Create an finite heightfield.
dGeomHeightfieldDataBuildCallback( heightid, NULL, near_heightfield_callback,
HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
// Give some very bounds which, while conservative,
// makes AABB computation more accurate than +/-INF.
//dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
gheight = dCreateHeightfield( space, heightid, 1 );
// Rotate so Z is up, not Y (which is the default orientation)
dMatrix3 R;
dRSetIdentity( R );
dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
dGeomSetRotation( gheight, R );
dGeomSetPosition( gheight, 50,50,0 );
// for (int i=0;i<MAX_ITEMS;i++) {
// items.push_back(generateItem());
//}
generateItems();
bulldozer_space = dSimpleSpaceCreate(space);
dSpaceSetCleanup (bulldozer_space,0);
bulldozer=new BoxItem(world,bulldozer_space,LENGTH,WIDTH,HEIGHT,CMASS);
bulldozer->setPosition(STARTX,STARTY,STARTZ);
bulldozer_cabin=new BoxItem(world,bulldozer_space,LENGTH/2,WIDTH/2,2*HEIGHT,CMASS/3);
bulldozer_cabin->setPosition(-LENGTH/4+STARTX,STARTY,3.0/2.0*HEIGHT+STARTZ);
bulldozer_bucket_c=new BoxItem(world,bulldozer_space,BUCKET_LENGTH,BUCKET_WIDTH,BUCKET_HEIGHT,CMASS/10);
bulldozer_bucket_c->setPosition(LENGTH/2+BUCKET_LENGTH/2+RADIUS+STARTX,STARTY,STARTZ);
bulldozer_bucket_l=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
bulldozer_bucket_l->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,-BUCKET_WIDTH/2+BUCKET_LENGTH/2+STARTY,STARTZ);
bulldozer_bucket_r=new BoxItem(world,bulldozer_space,BUCKET_WIDTH/5,BUCKET_LENGTH,BUCKET_HEIGHT,CMASS/20);
bulldozer_bucket_r->setPosition(LENGTH/2+BUCKET_LENGTH+RADIUS+BUCKET_WIDTH/10+STARTX,BUCKET_WIDTH/2-BUCKET_LENGTH/2+STARTY,STARTZ);
for (int i=0; i<4; i++) {
dQuaternion q;
dQFromAxisAndAngle(q,1,0,0,M_PI*0.5);
wheels[i] = new WheelItem(world,bulldozer_space,q,RADIUS,WMASS);
}
dBodySetPosition (wheels[0]->body,0.5*LENGTH+STARTX,WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[1]->body,0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[2]->body,-0.5*LENGTH+STARTX, WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
dBodySetPosition (wheels[3]->body,-0.5*LENGTH+STARTX,-WIDTH*0.5+STARTY,STARTZ-HEIGHT*0.5);
cabin_joint=dJointCreateSlider(world,0);
dJointAttach(cabin_joint,bulldozer->body,bulldozer_cabin->body);
dJointSetSliderAxis(cabin_joint,0,0,1);
dJointSetSliderParam(cabin_joint,dParamLoStop,0);
dJointSetSliderParam(cabin_joint,dParamHiStop,0);
bucket_joint_c=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_c,bulldozer->body,bulldozer_bucket_c->body);
dJointSetSliderAxis(bucket_joint_c,0,0,1);
dJointSetSliderParam(bucket_joint_c,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_c,dParamHiStop,0);
bucket_joint_l=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_l,bulldozer->body,bulldozer_bucket_l->body);
dJointSetSliderAxis(bucket_joint_l,0,0,1);
dJointSetSliderParam(bucket_joint_l,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_l,dParamHiStop,0);
bucket_joint_r=dJointCreateSlider(world,0);
dJointAttach(bucket_joint_r,bulldozer->body,bulldozer_bucket_r->body);
dJointSetSliderAxis(bucket_joint_r,0,0,1);
dJointSetSliderParam(bucket_joint_r,dParamLoStop,0);
dJointSetSliderParam(bucket_joint_r,dParamHiStop,0);
// front and back wheel hinges
for (int i=0; i<4; i++) {
wheelJoints[i] = dJointCreateHinge2 (world,0);
dJointAttach (wheelJoints[i],bulldozer->body,wheels[i]->body);
const dReal *a = dBodyGetPosition (wheels[i]->body);
dJointSetHinge2Anchor (wheelJoints[i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (wheelJoints[i],0,0,1);
dJointSetHinge2Axis2 (wheelJoints[i],0,1,0);
}
// seeting ERP & CRM
for (int i=0; i<4; i++) {
dJointSetHinge2Param (wheelJoints[i],dParamSuspensionERP,0.5);
dJointSetHinge2Param (wheelJoints[i],dParamSuspensionCFM,0.8);
}
// block back axis !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
for (int i=0; i<2; i++) {
dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0);
dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0);
}
}
void box::draw(unsigned int *buffer, unsigned int shader, float cameraX, float cameraY, float cameraZ, float cameraAzimuth, float cameraAltitude)
{
const dReal* tempRotMatrix = dBodyGetRotation(body);
const dReal* tempPosMatrix = dBodyGetPosition(body);
// use box' rotation for modelViewMatrix
GLfloat modelViewMatrix[16] = {
tempRotMatrix[0], tempRotMatrix[4], tempRotMatrix[8], 0.0,
tempRotMatrix[1], tempRotMatrix[5], tempRotMatrix[9], 0.0,
tempRotMatrix[2], tempRotMatrix[6], tempRotMatrix[10], 0.0,
tempPosMatrix[0], tempPosMatrix[1], tempPosMatrix[2], 1.0 };
glUniformMatrix4fv(
glGetUniformLocation(shader, "modelViewMatrix"),
1, false, modelViewMatrix);
GLfloat vertex_buffer_data[32] = { -sides[0]/2,-sides[1]/2,-sides[2]/2,1.0,
sides[0]/2,-sides[1]/2,-sides[2]/2,1.0,
sides[0]/2,sides[1]/2,-sides[2]/2,1.0,
-sides[0]/2,sides[1]/2,-sides[2]/2,1.0,
-sides[0]/2,-sides[1]/2,sides[2]/2,1.0,
sides[0]/2,-sides[1]/2,sides[2]/2,1.0,
sides[0]/2,sides[1]/2,sides[2]/2,1.0,
-sides[0]/2,sides[1]/2,sides[2]/2,1.0};
GLfloat normal_buffer_data[24] = { -sides[0],-sides[1],-sides[2],
sides[0],-sides[1],-sides[2],
sides[0],sides[1],-sides[2],
-sides[0],sides[1],-sides[2],
-sides[0],-sides[1],sides[2],
sides[0],-sides[1],sides[2],
sides[0],sides[1],sides[2],
-sides[0],sides[1],sides[2]};
GLuint element_buffer_data[36] = {0,1,2,
0,2,3,
0,3,4,
3,4,7,
3,2,7,
2,6,7,
4,5,7,
5,6,7,
0,1,4,
5,1,4,
1,2,5,
2,5,6};
// Load vertex data into buffer
glBindBuffer(GL_ARRAY_BUFFER, buffer[0]);
glBufferData(GL_ARRAY_BUFFER, sizeof(vertex_buffer_data), vertex_buffer_data, GL_STATIC_DRAW);
// Load normal data into buffer
glBindBuffer(GL_ARRAY_BUFFER, buffer[1]);
glBufferData(GL_ARRAY_BUFFER, sizeof(normal_buffer_data), normal_buffer_data, GL_STATIC_DRAW);
// Load index data into buffer
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, buffer[2]);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(element_buffer_data), element_buffer_data, GL_STATIC_DRAW);
// Bind attributes for position and normal
GLuint positionAttribute = glGetAttribLocation(shader, "vertexPos_modelspace");
glEnableVertexAttribArray(positionAttribute);
glBindBuffer(GL_ARRAY_BUFFER, buffer[0]);
glVertexAttribPointer(positionAttribute, 4, GL_FLOAT, GL_FALSE, 0, (void*)0);
GLuint normalAttribute = glGetAttribLocation(shader, "vertexNormal_modelspace");
glEnableVertexAttribArray(normalAttribute);
glBindBuffer(GL_ARRAY_BUFFER, buffer[1]);
glVertexAttribPointer(normalAttribute, 3, GL_FLOAT, GL_FALSE, 0, (void*)0);
// assign uniform data
glUniform1f(glGetUniformLocation(shader, "aspect"), 1);
glUniform1f(glGetUniformLocation(shader, "cotFOVy"), 1/tan(1.5/2));
glUniform1f(glGetUniformLocation(shader, "near"), .5);
glUniform1f(glGetUniformLocation(shader, "cameraX"), cameraX);
glUniform1f(glGetUniformLocation(shader, "cameraY"), cameraY);
glUniform1f(glGetUniformLocation(shader, "cameraZ"), cameraZ);
glUniform1f(glGetUniformLocation(shader, "azimuth"), cameraAzimuth);
glUniform1f(glGetUniformLocation(shader, "altitude"), cameraAltitude);
glUniform1f(glGetUniformLocation(shader, "far"), 40);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, buffer[2]);
glDrawElements(GL_TRIANGLES, sizeof(element_buffer_data) / sizeof(GLuint), GL_UNSIGNED_INT, (void*)0);
glDisableVertexAttribArray(positionAttribute);
// delete tempRotMatrix;
// delete tempPosMatrix;
}
void
dxJointTransmission::getInfo2( dReal worldFPS,
dReal /*worldERP*/,
const Info2Descr* info )
{
dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
const dReal *p[2], *omega[2];
int i;
// Transform all needed quantities to the global frame.
for (i = 0 ; i < 2 ; i += 1) {
dBodyGetRelPointPos(node[i].body,
anchors[i][0], anchors[i][1], anchors[i][2],
a[i]);
dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
n[i]);
p[i] = dBodyGetPosition(node[i].body);
omega[i] = dBodyGetAngularVel(node[i].body);
}
if (update) {
// Make sure both gear reference frames end up with the same
// handedness.
if (dCalcVectorDot3(n[0], n[1]) < 0) {
dNegateVector3(axes[0]);
dNegateVector3(n[0]);
}
}
// Calculate the mesh geometry based on the current mode.
switch (mode) {
case dTransmissionParallelAxes:
// Simply calculate the contact point as the point on the
// baseline that will yield the correct ratio.
dIASSERT (ratio > 0);
dSubtractVectors3(d, a[1], a[0]);
dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
dCopyVector3(c[1], c[0]);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
dCalcVectorCross3(l[i], d, n[i]);
}
break;
case dTransmissionIntersectingAxes:
// Calculate the line of intersection between the planes of the
// gears.
dCalcVectorCross3(l[0], n[0], n[1]);
dCopyVector3(l[1], l[0]);
nn = dCalcVectorDot3(n[0], n[1]);
dIASSERT(fabs(nn) != 1);
na_0 = dCalcVectorDot3(n[0], a[0]);
na_1 = dCalcVectorDot3(n[1], a[1]);
dAddScaledVectors3(O, n[0], n[1],
(na_0 - na_1 * nn) / (1 - nn * nn),
(na_1 - na_0 * nn) / (1 - nn * nn));
// Find the contact point as:
//
// c = ((r_a - O) . l) l + O
//
// where r_a the anchor point of either gear and l, O the tangent
// line direction and origin.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3(d, a[i], O);
m = dCalcVectorDot3(d, l[i]);
dAddScaledVectors3(c[i], O, l[i], 1, m);
}
break;
case dTransmissionChainDrive:
dSubtractVectors3(d, a[0], a[1]);
m = dCalcVectorLength3(d);
dIASSERT(m > 0);
// Caclulate the angle of the contact point relative to the
// baseline.
cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
// Calculate the contact radius in the local reference
// frame of the chain. This has axis x pointing along the
// baseline, axis y pointing along the sprocket axis and
// the remaining axis normal to both.
u[0] = radii[i] * cosphi;
u[1] = 0;
u[2] = radii[i] * sinphi;
// Transform the contact radius into the global frame.
dCalcVectorCross3(z, d, n[i]);
v[0] = dCalcVectorDot3(d, u);
v[1] = dCalcVectorDot3(n[i], u);
v[2] = dCalcVectorDot3(z, u);
// Finally calculate contact points and l.
dAddVectors3(c[i], a[i], v);
dCalcVectorCross3(l[i], v, n[i]);
dNormalize3(l[i]);
// printf ("%d: %f, %f, %f\n",
// i, l[i][0], l[i][1], l[i][2]);
}
break;
}
if (update) {
// We need to calculate an initial reference frame for each
// wheel which we can measure the current phase against. This
// frame will have the initial contact radius as the x axis,
// the wheel axis as the z axis and their cross product as the
// y axis.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], a[i]);
radii[i] = dCalcVectorLength3(r[i]);
dIASSERT(radii[i] > 0);
dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
reference[i]);
dNormalize3(reference[i]);
dCopyVector3(reference[i] + 8, axes[i]);
dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
// printf ("%f\n", dDOT(r[i], n[i]));
// printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
// reference[i][0],reference[i][1],reference[i][2],
// reference[i][4],reference[i][5],reference[i][6],
// reference[i][8],reference[i][9],reference[i][10]);
radii[i] = radii[i];
phase[i] = 0;
}
ratio = radii[0] / radii[1];
update = 0;
}
for (i = 0 ; i < 2 ; i += 1) {
dReal phase_hat;
dSubtractVectors3 (r[i], c[i], a[i]);
// Transform the (global) contact radius into the gear's
// reference frame.
dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
dMultiply0_331(t, reference[i], s);
// Now simply calculate its angle on the plane relative to the
// x-axis which is the initial contact radius. This will be
// an angle between -pi and pi that is coterminal with the
// actual phase of the wheel. To find the real phase we
// estimate it by adding omega * dt to the old phase and then
// find the closest angle to that, that is coterminal to
// theta.
theta = atan2(t[1], t[0]);
phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
if (phase_hat > M_PI_2) {
if (theta < 0) {
theta += (dReal)(2 * M_PI);
}
theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
} else if (phase_hat < -M_PI_2) {
if (theta > 0) {
theta -= (dReal)(2 * M_PI);
}
theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
}
if (phase_hat - theta > M_PI) {
phase[i] = theta + (dReal)(2 * M_PI);
} else if (phase_hat - theta < -M_PI) {
phase[i] = theta - (dReal)(2 * M_PI);
} else {
phase[i] = theta;
}
dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
}
// Calculate the phase error. Depending on the mode the condition
// is that the distances traveled by each contact point must be
// either equal (chain and sprockets) or opposite (gears).
if (mode == dTransmissionChainDrive) {
delta = (dCalcVectorLength3(r[0]) * phase[0] -
dCalcVectorLength3(r[1]) * phase[1]);
} else {
delta = (dCalcVectorLength3(r[0]) * phase[0] +
dCalcVectorLength3(r[1]) * phase[1]);
}
// When in chain mode a torque reversal, signified by the change
// in sign of the wheel phase difference, has the added effect of
// switching the active chain branch. We must therefore reflect
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dIASSERT(a > 0);
dAddScaledVectors3(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
}
}
// Do not add the constraint if there's backlash and we're in the
// backlash gap.
if (backlash == 0 || fabs(delta) > backlash) {
// The constraint is satisfied iff the absolute velocity of the
// contact point projected onto the tangent of the wheels is equal
// for both gears. This velocity can be calculated as:
//
// u = v + omega x r_c
//
// The constraint therefore becomes:
// (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
// (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], p[i]);
}
dCalcVectorCross3(info->J1a, r[0], l[0]);
dCalcVectorCross3(info->J2a, l[1], r[1]);
dCopyVector3(info->J1l, l[0]);
dCopyNegatedVector3(info->J2l, l[1]);
if (delta > 0) {
if (backlash > 0) {
info->lo[0] = -dInfinity;
info->hi[0] = 0;
}
info->c[0] = -worldFPS * erp * (delta - backlash);
} else {
if (backlash > 0) {
info->lo[0] = 0;
info->hi[0] = dInfinity;
}
info->c[0] = -worldFPS * erp * (delta + backlash);
}
}
info->cfm[0] = cfm;
// printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
// Cache the contact point (in world coordinates) to avoid
// recalculation if requested by the user.
dCopyVector3(contacts[0], c[0]);
dCopyVector3(contacts[1], c[1]);
}
void PhysicsActor::update(double deltaTime){
if (drawType==DRAW_CUBE){
if (sceneData->controller->bRunning)
drawType=DRAW_NULL;
else
drawType=DRAW_CUBE;
}
const dReal *p = dBodyGetPosition(body);
//const dReal *p = dGeomGetPosition(geom);
const dReal *r = dBodyGetRotation(body);
float m[16];
m[ 0] = r[ 0];m[ 1] = r[ 4];m[ 2] = r[ 8];m[ 3] = 0;
m[ 4] = r[ 1];m[ 5] = r[ 5];m[ 6] = r[ 9];m[ 7] = 0;
m[ 8] = r[ 2];m[ 9] = r[ 6];m[10] = r[10];m[11] = 0;
m[12] = p[ 0];m[13] = p[ 1];m[14] = p[ 2];m[15] = 1;
//update physics from drawing when not live
// make sure we assign our base again if we had one when simulating
Matrix4f bGlobal;
bGlobal.identity();
if (!renderer->bUpdatePhysics){
if (oldBase && !base){
base=oldBase;
oldBase=NULL;
}
if (base){
bGlobal= renderer->inverseCameraMatrix * baseMatrix;// * originalMatrix;
dBodySetPosition(body,bGlobal.data[12],bGlobal.data[13],bGlobal.data[14]);
//dBodySetPosition(body,bGlobal.data[12] + transformMatrix.data[12],bGlobal.data[13] + transformMatrix.data[13],bGlobal.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]);
//calculate viewpoint into absolute position!
dBodySetPosition(body,bGlobal.data[12] + transformMatrix.data[12],bGlobal.data[13] + transformMatrix.data[13], bGlobal.data[14] +transformMatrix.data[14]);
}
//update drawing from physis when live
//also get rid of base actors as they screw up the calculation
//originalMatrix.identity();
}else{
if (!bFixToWorld){
if (base && !oldBase){
oldBase=base;
base=NULL;
}
transformMatrix=m;
}else{
dBodySetLinearVel(body,0,0,0);
if (base){
bGlobal= (renderer->inverseCameraMatrix * baseMatrix);// * originalMatrix;
//dBodySetPosition(body,bGlobal.data[12] + transformMatrix.data[12],bGlobal.data[13] + transformMatrix.data[13],bGlobal.data[14] + transformMatrix.data[14]);
dBodySetPosition(body,bGlobal.data[12],bGlobal.data[13],bGlobal.data[14]);
}else{
dBodySetPosition(body,bGlobal.data[12] + transformMatrix.data[12],bGlobal.data[13] + transformMatrix.data[13], bGlobal.data[14] +transformMatrix.data[14]);
}
}
}
}
void Physics::perframe(){
//dWorldStep(worldId);
return; //not implemented yet
#ifdef ODE
for(int i=0; i<level->colliders.size(); i++){
Object* obj=level->colliders[i];
if(level->colliders[i]->collideType=="box" || level->colliders[i]->collideType=="cube"){
if(level->colliders[i]->oldCollideType!="box" && level->colliders[i]->oldCollideType!="cube"){
obj->geomId=dCreateBox(spaceId,obj->collideParameters.x,obj->collideParameters.y,obj->collideParameters.z);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
if(obj->boxBuilt){
float lx=level->colliders[i]->box.px-level->colliders[i]->box.nx;
float ly=level->colliders[i]->box.py-level->colliders[i]->box.ny;
float lz=level->colliders[i]->box.pz-level->colliders[i]->box.nz;
dGeomBoxSetLengths (level->colliders[i]->geomId, lx, ly, lz);
}
}else if(obj->collideType=="sphere"){
if(obj->oldCollideType!="sphere"){
obj->geomId=dCreateSphere(spaceId,obj->collideParameters.x);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
}
}else if(obj->collideType=="plane"){
obj->geomId=dCreatePlane(spaceId,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x);
}else if(obj->collideType=="cylindar"){
obj->geomId=dCreateCCylinder(spaceId,obj->collideParameters.x,obj->collideParameters.y);
}else if(obj->collideType=="triangle"){
if(obj->oldCollideType!="triangle"){
dTriMeshDataID tridat=dGeomTriMeshDataCreate();
obj->geomId=dCreateTriMesh (spaceId, tridat,
NULL,
NULL,
NULL);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
}
if(obj->collide){
dGeomEnable(obj->geomId);
}else{
dGeomDisable(obj->geomId);
}
for(int j=0; j<level->colliders.size(); j++){
if(!level->colliders[i]->geomIdInit || !level->colliders[j]->geomIdInit){
continue;
}
dContactGeom contactg[2];
int cnt=dCollide(level->colliders[i]->geomId,level->colliders[j]->geomId,0,contactg,sizeof(dContactGeom));
if(cnt>0){
dSurfaceParameters surf;
surf.mode=0;
surf.mode=dContactBounce;
surf.mu=level->colliders[i]->friction;
surf.bounce=level->colliders[i]->bounce;
surf.bounce_vel=level->colliders[i]->bounceThreshold;
dContact contact;
contact.geom=contactg[0];
contact.surface=surf;
dJointID joint=dJointCreateContact(worldId,0,&contact);
if(!level->colliders[i]->dynamic){
dJointAttach(joint,level->colliders[j]->bodyId,0);
}else if(!level->colliders[j]->dynamic){
dJointAttach(joint,level->colliders[i]->bodyId,0);
}else{
dJointAttach(joint,level->colliders[i]->bodyId,level->colliders[j]->bodyId);
}
}
}
}
dWorldSetGravity(worldId,gravity.x,gravity.y,gravity.z);
dWorldQuickStep(worldId,1);
for(int i=0; i<level->dynamicObjects.size(); i++){
if(!level->dynamicObjects[i]->bodyIdInit){
continue;
}
if(level->dynamicObjects[i]->dynamic){
dBodyEnable(level->dynamicObjects[i]->bodyId);
}else{
dBodyDisable(level->dynamicObjects[i]->bodyId);
}
const dReal* pos=new dReal[3];
pos=dBodyGetPosition(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->pos.x=pos[0];
level->dynamicObjects[i]->pos.y=pos[1];
level->dynamicObjects[i]->pos.z=pos[2];
const dReal* rot=new dReal[3];
rot=dBodyGetRotation(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->rot.x=rot[0];
level->dynamicObjects[i]->rot.y=rot[1];
level->dynamicObjects[i]->rot.z=rot[2];
}
#endif
}
//===========================================================================
void cODEGenericBody::updateBodyPosition(void)
{
const double *odePosition;
const double *odeRotation;
// Retrieve position and orientation from ODE body.
if (m_ode_body != NULL)
{
odePosition = dBodyGetPosition(m_ode_body);
odeRotation = dBodyGetRotation(m_ode_body);
}
else
{
return;
}
// set new position
m_localPos.set(odePosition[0],odePosition[1],odePosition[2]);
// set new orientation
m_localRot.set(odeRotation[0],odeRotation[1],odeRotation[2],
odeRotation[4],odeRotation[5],odeRotation[6],
odeRotation[8],odeRotation[9],odeRotation[10]);
// store previous position if object is a mesh
if (m_ode_triMeshDataID != NULL)
{
m_prevTransform[0] = odeRotation[0];
m_prevTransform[1] = odeRotation[4];
m_prevTransform[2] = odeRotation[8];
m_prevTransform[3] = 0.0;
m_prevTransform[4] = odeRotation[1];
m_prevTransform[5] = odeRotation[5];
m_prevTransform[6] = odeRotation[9];
m_prevTransform[7] = 0.0;
m_prevTransform[8] = odeRotation[2];
m_prevTransform[9] = odeRotation[6];
m_prevTransform[10] = odeRotation[10];
m_prevTransform[11] = 0.0;
m_prevTransform[12] = odePosition[0];
m_prevTransform[13] = odePosition[1];
m_prevTransform[14] = odePosition[2];
m_prevTransform[15] = 1.0;
dGeomTriMeshSetLastTransform(m_ode_geom, m_prevTransform);
}
// Normalize frame
// This can be useful is ODE is running in SINGLE precision mode
// where precision is a problem
/*
cVector3d c0(odeRotation[0], odeRotation[4], odeRotation[8]);
cVector3d c1(odeRotation[1], odeRotation[5], odeRotation[9]);
cVector3d c2(odeRotation[2], odeRotation[6], odeRotation[10]);
c0.crossr(c1, c2);
c2.crossr(c0, c1);
c0.normalize();
c1.normalize();
c2.normalize();
// set new orientation
m_localRot.setCol(c0, c1, c2);
*/
}
int main (int argc, char **argv)
{
initscr();
dInitODE();
// setup world
world = dWorldCreate();
dWorldSetGravity(world, 0, 0, -9.8);
dSpaceID space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
// grand plane (for collision)
dGeomID ground = dCreatePlane(space, 0, 0, 1, 0);
// body
dBodyID ball = dBodyCreate(world);
// mass
dMass m;
dMassSetZero(&m);
const dReal radius = 0.2; // 20cm
const dReal mass = 1.0; // 1kg
dMassSetSphereTotal(&m, mass, radius);
dBodySetMass(ball, &m);
dBodySetPosition(ball, 0.0, 0.0, 10); // x=0m, y=0m, z=10m
dGeomID geom = dCreateSphere(space, radius);
dGeomSetBody(geom, ball);
// simulation loop (1000 step)
dReal stepsize = 0.01; // 0.01ms
for (int i = 0; i < 1000; ++i) {
dSpaceCollide(space, 0, &nearCallback);
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup);
// draw
erase();
const dReal *pos = dBodyGetPosition(ball);
const dReal *R = dBodyGetRotation(ball);
mvprintw((int)(12-pos[2]), 5, "*"); // ball
mvprintw(12, 0, "============================"); // ground
// draw
move(0, 0);
printw("t=%f, pos=(%f, %f, %f) \n", stepsize * i, pos[0], pos[1], pos[2]);
refresh();
usleep(10 * 1000);
}
// cleanup
dWorldDestroy(world);
dCloseODE();
endwin();
return 0;
}
Ogre::Vector3 GameObject::GetLocation()
{
const dReal *pos = dBodyGetPosition(m_body);
return Ogre::Vector3(pos[0], pos[1], pos[2]);
}
/* returns the vector containing the position of the body */
void body_position (t_real *res, dBodyID b) {
dCopyVector3 (res, dBodyGetPosition (b));
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dReal maxheight = 0;
for (k=0; k<num; k++)
{
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRSetIdentity (R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
//<---- Convex Object
else if (cmd == 'v')
{
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
obj[i].geom[0] = dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
#else
obj[i].geom[0] = dCreateConvex (space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x' && USE_GEOM_OFFSET) {
setBody = 1;
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule (space,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate (&m2,drot[k]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition (obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// Translation *after* rotation
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<GPB; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == 'p'&& selected >= 0)
{
const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
rot[0],rot[1],rot[2],rot[3],
rot[4],rot[5],rot[6],rot[7],
rot[8],rot[9],rot[10],rot[11]);
}
else if (cmd == 'f' && selected >= 0 && selected < num) {
if (dBodyIsEnabled(obj[selected].body))
doFeedback = 1;
}
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == 'o') {
draw_geom ^= 1;
}
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
//<---- Convex Object
else if (type == dConvexClass)
{
#if 0
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
#else
dsDrawConvex(pos,R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition (body);
const dReal *bodyr = dBodyGetRotation (body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void makeCar(dReal x, dReal y, int &bodyI, int &jointI, int &boxI, int &sphereI)
{
int i;
dMass m;
// chassis body
body[bodyI] = dBodyCreate (world);
dBodySetPosition (body[bodyI],x,y,STARTZ);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (body[bodyI],&m);
box[boxI] = dCreateBox (space,LENGTH,WIDTH,HEIGHT);
dGeomSetBody (box[boxI],body[bodyI]);
// wheel bodies
for (i=1; i<=4; i++) {
body[bodyI+i] = dBodyCreate (world);
dQuaternion q;
dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
dBodySetQuaternion (body[bodyI+i],q);
dMassSetSphere (&m,1,RADIUS);
dMassAdjust (&m,WMASS);
dBodySetMass (body[bodyI+i],&m);
sphere[sphereI+i-1] = dCreateSphere (space,RADIUS);
dGeomSetBody (sphere[sphereI+i-1],body[bodyI+i]);
}
dBodySetPosition (body[bodyI+1],x+0.4*LENGTH-0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+2],x+0.4*LENGTH-0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+3],x-0.4*LENGTH+0.5*RADIUS,y+WIDTH*0.5,STARTZ-HEIGHT*0.5);
dBodySetPosition (body[bodyI+4],x-0.4*LENGTH+0.5*RADIUS,y-WIDTH*0.5,STARTZ-HEIGHT*0.5);
// front and back wheel hinges
for (i=0; i<4; i++) {
joint[jointI+i] = dJointCreateHinge2 (world,0);
dJointAttach (joint[jointI+i],body[bodyI],body[bodyI+i+1]);
const dReal *a = dBodyGetPosition (body[bodyI+i+1]);
dJointSetHinge2Anchor (joint[jointI+i],a[0],a[1],a[2]);
dJointSetHinge2Axis1 (joint[jointI+i],0,0,(i<2 ? 1 : -1));
dJointSetHinge2Axis2 (joint[jointI+i],0,1,0);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionERP,0.8);
dJointSetHinge2Param (joint[jointI+i],dParamSuspensionCFM,1e-5);
dJointSetHinge2Param (joint[jointI+i],dParamVel2,0);
dJointSetHinge2Param (joint[jointI+i],dParamFMax2,FMAX);
}
//center of mass offset body. (hang another copy of the body COMOFFSET units below it by a fixed joint)
dBodyID b = dBodyCreate (world);
dBodySetPosition (b,x,y,STARTZ+COMOFFSET);
dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
dMassAdjust (&m,CMASS/2.0);
dBodySetMass (b,&m);
dJointID j = dJointCreateFixed(world, 0);
dJointAttach(j, body[bodyI], b);
dJointSetFixed(j);
//box[boxI+1] = dCreateBox(space,LENGTH,WIDTH,HEIGHT);
//dGeomSetBody (box[boxI+1],b);
bodyI += 5;
jointI += 4;
boxI += 1;
sphereI += 4;
}
const dReal* WorldPhysics::getBulldozerPosition() {
return dBodyGetPosition(bulldozer->body);
}
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
dWorldStepFast1 (world,0.05,ITERS);
#endif
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
State* init() {
State* state = new State();
dInitODE();
SDL_Init(SDL_INIT_EVERYTHING);
state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL);
SDL_WM_SetCaption("Physics", NULL);
SDL_Flip(state->screen);
SDL_ShowCursor(SDL_DISABLE);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
GLfloat light_ambient[] = { 1, 1, 1, 1 };
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
GLfloat lightpos[] = {0, 4, 0, 1};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
glShadeModel(GL_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glClearColor(0, 0, 0, 1);
state->posx = 0;//21;
state->posy = 4;//8;
state->posz = 5;//21;
state->rotx = 0;
state->roty = 0;//-40;
state->rotz = 0;
state->wkey = false;
state->akey = false;
state->skey = false;
state->dkey = false;
state->gkey = false;
state->simSpeed = 60;
state->carcam = false;
state->carbodydrawable = new Drawable("objs/carbody.obj");
state->carwheeldrawable = new Drawable("objs/carwheel.obj");
state->map = new Drawable("objs/jump2.obj");
state->cube = new Drawable("objs/cube.obj");
// state->monkey = new Drawable("objs/monkey.obj");
state->world = dWorldCreate();
dWorldSetGravity(state->world, 0, -9.8, 0);
state->worldSpace = dHashSpaceCreate(0);
const double carHeight = 0.65;
const double carZ = 90;
const double carX = 0;
const float speed = -1000;
const float force = 200;
state->carbodybody = dBodyCreate(state->world);
dBodySetPosition(state->carbodybody, carX, carHeight, carZ);
dMass bodymass;
dMassSetBoxTotal(&bodymass, 100, 2, 4, 1);
dBodySetMass(state->carbodybody, &bodymass);
dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4);
dGeomSetBody(carbodygeom, state->carbodybody);
state->flcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2);
const dMatrix3 m = { 0, 0, 1, 0
, 0, 1, 0, 0
, 0, 0, 1, 0 };
dBodySetRotation(state->flcarwheelbody, m);
dMass wheelmass;
dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2);
dBodySetMass(state->flcarwheelbody, &wheelmass);
dJointID joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->flcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(flcarwheelgeom, state->flcarwheelbody);
dJointID motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->flcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->frcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2);
dBodySetRotation(state->frcarwheelbody, m);
dBodySetMass(state->frcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->frcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(frcarwheelgeom, state->frcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->frcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->blcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->blcarwheelbody, m);
dBodySetMass(state->blcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->blcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(blcarwheelgeom, state->blcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->blcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->brcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->brcarwheelbody, m);
dBodySetMass(state->brcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->brcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(brcarwheelgeom, state->brcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->brcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->var = new double[3];
state->var = dBodyGetPosition(state->carbodybody);
// cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
//TODO check if translation matrix from dBody can be used.
dSpaceID cubespace = dHashSpaceCreate(state->worldSpace);
for(int i = 0; i < NUM_CUBES/10; i++) {
for(int k = 0; k < 10; k++) {
state->cubebody[i*10+k] = dBodyCreate(state->world);
dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1);
dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70);
dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2);
dGeomSetBody(cubegeom, state->cubebody[i*10+k]);
}
}
{
int indexSize = state->map->vertices.size()/3;
unsigned int* index = new unsigned int[indexSize];
for(int i = 0; i < indexSize; i++)
index[i] = i;
dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12);
dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
dGeomSetPosition(mapGeom, 0, 0, 0);
}
// state->monkeyBody = dBodyCreate(state->world);
// {
// int indexSize = state->monkey->vertices.size()/3;
// unsigned int* index = new unsigned int[indexSize];
// for(int i = 0; i < indexSize; i++)
// index[i] = i;
// dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
// dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12);
// dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
// dGeomSetPosition(monkeyGeom, 0, 2, 0);
// dGeomSetBody(monkeyGeom, state->monkeyBody);
// }
state->physicsContactgroup = dJointGroupCreate(0);
return state;
}
