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