short base::check_state(void) { if(body) dBodyGetRelPointVel (body, 0, 0, 0, pointrelvel); if (pointrelvel[1] > -1 && pointrelvel[1] < 1) { stand++; if (stand>=1 && c_tmp==0) Dup=1, c_tmp=1; //if >=1 enable dj if (stand>=10) Tup=0, Dup=0, c_tmp=0; //enable dj on floor //stand>=X --X time for set stand if (Dup==1) Tup=2, Dup=0; } else stand=0; if(body) dBodyGetRelPointVel(body,0,0,0,pointrelvel); if (pointrelvel[1] > 1 || pointrelvel[1] < -1) { fly++; if (fly>=8) Tup=1, Dup=0; //fly>=X where X is time to keep not_jump state } else fly=0; return Tup; //1 - first j. 2- dj. 0 - floor }
dReal dJointGetPRPositionRate( dJointID j ) { dxJointPR* joint = ( dxJointPR* ) j; dUASSERT( joint, "bad joint argument" ); checktype( joint, PR ); // get axis1 in global coordinates dVector3 ax1; dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 ); if ( joint->node[1].body ) { dVector3 lv2; dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 ); return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 ); } else { dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel ); return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate); } }
void base::passive_control(void) { float tmp; up=check_state(); dBodyGetRelPointVel(body,0,0,0,pointrelvel); //get body's speed if(last>1 && !up) tmp=pointrelvel[0]/speed; else if(!up)tmp=0.03; //stand anim speed else tmp=0.3; //jump anim speed if(tmp>=0)calc_speed+=tmp; else calc_speed+=(tmp*-1); if(up) { if(last==0 || last==2) last=4; if(last==1 || last==3) last=5; } if(!up) //idle dBodyAddForce(body,-pointrelvel[0]/1.5,0,0); //for circle only if(pointrelvel[0]<=1 && pointrelvel[0]>=-1 && !up) //stand animation if(last!=0 && last!=1) last-=2; if(calc_speed>=1) { translated_val+=(double)1/texture[last].n; calc_speed=0; } if(last_prev!=last) translated_val=0; last_prev=last; //~ hack_2d(); //~ if(keystates['e']) rot+=5; //~ if(keystates['q']) rot-=5; }
void base::move(int size, int max) { //for wall-up i must be a sphere with total-mass, torq in air and in stand, and with force in air if(body) dBodyGetRelPointVel(body,0,0,0,pointrelvel); // const dReal *tr = dBodyGetTorque(body); if (up){ //in up if (size > 0 && pointrelvel[0]<max) dBodyAddForce (body,size/10,0,0); // if (size > 0 && pointrelvel[0]<max) dBodyAddTorque (body,size/4, 0, 0); else if (size < 0 && pointrelvel[0]>-max) dBodyAddForce (body,size/10,0,0); // else if (size < 0 && pointrelvel[0]>-max) dBodyAddTorque (body,size,0,0); } else{ if (size > 0 && pointrelvel[0] < max) { dBodyAddForce (body,size,0,0); // dBodyAddRelTorque (body, 0, 0, -size*60); } else if (size < 0 && pointrelvel[0] > -max) { dBodyAddForce (body,size,0,0); // dBodyAddTorque (body, 0, 0, -size*60); } //~ if (up) //in up //~ { //~ if (size > 0 && pointrelvel[0]<max) dBodyAddForce (body,size/4,0,0); //~ else if (size < 0 && pointrelvel[0]>-max) dBodyAddForce (body,size/4,0,0); //~ } //~ //~ else //~ { //~ if (size > 0 && pointrelvel[0] < max) dBodyAddForce (body, size,0,0); //~ else if (size < 0 && pointrelvel[0] > -max) dBodyAddForce (body,size,0,0); //~ } }
/* returns a vector that expresses the angular velocity of a point that has coordinates * (x, y, z) in the material reference frame (u, v, t) */ void body_point_velocity (t_real *res, dBodyID b, t_real x, t_real y, t_real z) { dBodyGetRelPointVel (b, x, y, z, res); }
//collide camera with track, generate acceleration on camera if collisding void camera_physics_step() { //some values that are easy to deal with: dReal time = internal.stepsize; car_struct *car = camera.car; camera_settings *settings = camera.settings; //if camera got a targeted car and proper settings, simulate movment // //divided into 4 parts: //1) calculate velocity //2) check for collisions //3) add damping to velocity //4) move camera // if (car && settings) { //random values will come handy: //check for some exceptions if (settings->reverse) //enabled { if (car->throttle > 0.0) //wanting to go forward camera.reverse = false; else if (car->throttle < 0.0 && car->velocity < 0.0) //wanting and going backwards camera.reverse = true; } if (settings->in_air) //in air enabled { if (!(car->sensor1->event) && !(car->sensor2->event)) //in air { if (camera.in_air) //in ground mode { //smooth transition between offset and center (and needed) if (settings->offset_scale_speed != 0 && camera.offset_scale > 0) camera.offset_scale -= (settings->offset_scale_speed*time); else //jump directly camera.offset_scale = 0; } if (!camera.in_air) //camera not in "air mode" { if (camera.air_timer > settings->air_time) { camera.in_air = true; //go to air mode camera.air_timer = 0; //reset timer } else camera.air_timer += time; } } else //not in air { if (camera.in_air) //camera in "air mode" { if (camera.air_timer > settings->ground_time) { camera.in_air = false; //leave air mode camera.air_timer = 0; //reset timer } else camera.air_timer += time; } else //camera in "ground mode" { //smooth transition between center and offset (and needed) if (settings->offset_scale_speed != 0 && camera.offset_scale < 1) camera.offset_scale += (settings->offset_scale_speed*time); else //jump directly camera.offset_scale = 1; } } } //store old velocity dReal old_vel[3] = {camera.vel[0], camera.vel[1], camera.vel[2]}; //wanted position of "target" - position on car that should be focused dVector3 t_pos; //wanted position of camera relative to anchor (translated to world coords) dVector3 pos_wanted; if (camera.reverse && !camera.in_air) //move target and position to opposite side (if not just spinning in air) { dBodyGetRelPointPos (car->bodyid, settings->target[0], -settings->target[1], settings->target[2]*car->dir, t_pos); dBodyVectorToWorld(car->bodyid, settings->distance[0], -settings->distance[1], settings->distance[2]*car->dir, pos_wanted); } else //normal { dBodyGetRelPointPos (car->bodyid, settings->target[0]*camera.offset_scale, settings->target[1]*camera.offset_scale, settings->target[2]*car->dir*camera.offset_scale, t_pos); dBodyVectorToWorld(car->bodyid, settings->distance[0], settings->distance[1], settings->distance[2]*car->dir, pos_wanted); } //position and velocity of anchor dVector3 a_pos; dBodyGetRelPointPos (car->bodyid, settings->anchor[0], settings->anchor[1], settings->anchor[2]*car->dir, a_pos); //relative pos and vel of camera (from anchor) dReal pos[3] = {camera.pos[0]-a_pos[0], camera.pos[1]-a_pos[1], camera.pos[2]-a_pos[2]}; //vector lengths dReal pos_l = v_length(pos[0], pos[1], pos[2]); //how far from car we want to stay //(TODO: could be computed just once - only when changing camera) dReal pos_wanted_l = v_length(pos_wanted[0], pos_wanted[1], pos_wanted[2]); //unit vectors dReal pos_u[3] = {pos[0]/pos_l, pos[1]/pos_l, pos[2]/pos_l}; dReal pos_wanted_u[3] = {pos_wanted[0]/pos_wanted_l, pos_wanted[1]/pos_wanted_l, pos_wanted[2]/pos_wanted_l}; // // 1) spring physics for calculating acceleration // //"linear spring" between anchor and camera (based on distance) dReal dist = pos_l-pos_wanted_l; if (settings->linear_stiffness == 0) //disabled smooth movement, jump directly { //chanses are we have an anchor distance of 0, then vel=0 if (pos_wanted_l == 0) { //position at wanted camera.pos[0]=a_pos[0]; camera.pos[1]=a_pos[1]; camera.pos[2]=a_pos[2]; //velocity 0 camera.vel[0]=0; camera.vel[1]=0; camera.vel[2]=0; } else { //set position camera.pos[0]-=pos_u[0]*dist; camera.pos[1]-=pos_u[1]*dist; camera.pos[2]-=pos_u[2]*dist; //velocity towards/from anchor = 0 //vel towards anchor dReal dot = (pos_u[0]*camera.vel[0] + pos_u[1]*camera.vel[1] + pos_u[2]*camera.vel[2]); //remove vel towards anchor camera.vel[0]-=pos_u[0]*dot; camera.vel[1]-=pos_u[1]*dot; camera.vel[2]-=pos_u[2]*dot; } } else //smooth movement { //how much acceleration (based on distance from wanted distance) dReal acceleration = time*(camera.settings->linear_stiffness)*dist; camera.vel[0]-=pos_u[0]*acceleration; camera.vel[1]-=pos_u[1]*acceleration; camera.vel[2]-=pos_u[2]*acceleration; } //perpendicular "angular spring" to move camera behind car if (pos_wanted_l > 0 && !camera.in_air) //actually got distance, and camera not in "air mode" { //dot between wanted and current rotation dReal dot = (pos_wanted_u[0]*pos_u[0] + pos_wanted_u[1]*pos_u[1] + pos_wanted_u[2]*pos_u[2]); if (dot < 1.0) //if we aren't exactly at wanted position (and prevent possibility of acos a number bigger than 1.0) { //angle dReal angle = acos(dot); //how much acceleration dReal accel = time*angle*(settings->angular_stiffness); //direction of acceleration (remove part of wanted that's along current pos) dReal dir[3]; dir[0]=pos_wanted_u[0]-dot*pos_u[0]; dir[1]=pos_wanted_u[1]-dot*pos_u[1]; dir[2]=pos_wanted_u[2]-dot*pos_u[2]; //not unit, get length and modify accel to compensate for not unit accel /= v_length(dir[0], dir[1], dir[2]); camera.vel[0]+=(accel*dir[0]); camera.vel[1]+=(accel*dir[1]); camera.vel[2]+=(accel*dir[2]); } } // // 2) check for collision, and if so, remove possible movement into collision direction // if (settings->radius > 0) { dGeomID geom = dCreateSphere (0, settings->radius); dGeomSetPosition(geom, camera.pos[0], camera.pos[1], camera.pos[2]); dContactGeom contact[internal.contact_points]; int count = dCollide ( (dGeomID)(track.object->space), geom, internal.contact_points, &contact[0], sizeof(dContactGeom)); int i; dReal depth; dReal V; for (i=0; i<count; ++i) { depth = contact[i].depth; camera.pos[0]-=contact[i].normal[0]*depth; camera.pos[1]-=contact[i].normal[1]*depth; camera.pos[2]-=contact[i].normal[2]*depth; //remove movement into colliding object //velocity along collision axis V = camera.vel[0]*contact[i].normal[0] + camera.vel[1]*contact[i].normal[1] + camera.vel[2]*contact[i].normal[2]; if (V > 0) //right direction (not away from collision)? { //remove direction camera.vel[0]-=V*contact[i].normal[0]; camera.vel[1]-=V*contact[i].normal[1]; camera.vel[2]-=V*contact[i].normal[2]; } } dGeomDestroy (geom); } // // 3) damping of current velocity // if (settings->relative_damping) { //damping (of relative movement) dVector3 a_vel; //anchor velocity dBodyGetRelPointVel (car->bodyid, settings->anchor[0], settings->anchor[1], settings->anchor[2]*car->dir, a_vel); dReal vel[3] = {camera.vel[0]-a_vel[0], camera.vel[1]-a_vel[1], camera.vel[2]-a_vel[2]}; //velocity relative to anchor dReal damping = (time*settings->damping); if (damping > 1) damping=1; camera.vel[0]-=damping*vel[0]; camera.vel[1]-=damping*vel[1]; camera.vel[2]-=damping*vel[2]; } else { //absolute damping dReal damping = 1-(time*settings->damping); if (damping < 0) damping=0; camera.vel[0]*=damping; camera.vel[1]*=damping; camera.vel[2]*=damping; } // // 4) movement // //during the step, camera will have linear acceleration from old velocity to new //avarge velocity over the step is between new and old velocity camera.pos[0]+=((camera.vel[0]+old_vel[0])/2)*time; camera.pos[1]+=((camera.vel[1]+old_vel[1])/2)*time; camera.pos[2]+=((camera.vel[2]+old_vel[2])/2)*time; //movement of camera done. // //the following is smooth rotation and focusing // //smooth rotation (if enabled) //(move partially from current "up" to car "up", and make unit) dReal target_up[3]; if (camera.in_air) //if in air, use absolute up instead { target_up[0] = 0; target_up[1] = 0; target_up[2] = 1; } else //use car up { const dReal *rotation = dBodyGetRotation (car->bodyid); target_up[0] = rotation[2]*car->dir; target_up[1] = rotation[6]*car->dir; target_up[2] = rotation[10]*car->dir; } if (settings->rotation_tightness == 0) //disabled, rotate directly { camera.up[0]=target_up[0]; camera.up[1]=target_up[1]; camera.up[2]=target_up[2]; } else { dReal diff[3]; //difference between diff[0]=target_up[0]-camera.up[0]; diff[1]=target_up[1]-camera.up[1]; diff[2]=target_up[2]-camera.up[2]; dReal movement=time*(settings->rotation_tightness); if (movement > 1) movement=1; camera.up[0]+=diff[0]*movement; camera.up[1]+=diff[1]*movement; camera.up[2]+=diff[2]*movement; //gluLookAt wants up to be unit dReal length=v_length(camera.up[0], camera.up[1], camera.up[2]); camera.up[0]/=length; camera.up[1]/=length; camera.up[2]/=length; } //smooth movement of target focus (if enabled) if (settings->target_tightness == 0) { camera.t_pos[0] = t_pos[0]; camera.t_pos[1] = t_pos[1]; camera.t_pos[2] = t_pos[2]; } else { dReal diff[3], movement; diff[0]=t_pos[0]-camera.t_pos[0]; diff[1]=t_pos[1]-camera.t_pos[1]; diff[2]=t_pos[2]-camera.t_pos[2]; movement = time*(settings->target_tightness); if (movement>1) movement=1; camera.t_pos[0]+=diff[0]*movement; camera.t_pos[1]+=diff[1]*movement; camera.t_pos[2]+=diff[2]*movement; } } }
void PhysicsBody::getRelPointVel(const Vec3f &v, Vec3f &result) { dVector3 t; dBodyGetRelPointVel(_BodyID, v.x(), v.y(), v.z(), t); result.setValue(Vec3f(t[0], t[1], t[2])); }
void ODE_Link::getLinearVel(hrp::Vector3& v){ dVector3 result; dBodyGetRelPointVel(bodyId, -C[0], -C[1], -C[2], result); v << result[0], result[1], result[2]; }
void base::hack_2d(void) { const dReal *rot = dBodyGetAngularVel (body); const dReal *quat_ptr; dReal quat[4], quat_len; quat_ptr = dBodyGetQuaternion (body); quat[0] = quat_ptr[0]; quat[1] = 0; quat[2] = 0; quat[3] = quat_ptr[3]; quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]); quat[0] /= quat_len; quat[3] /= quat_len; dBodySetQuaternion (body, quat); dBodySetAngularVel (body, 0, 0, rot[2]); //Restricting Rotation To One Axis //The plane2D stops objects rotating along non-2d axes, //but it does not compensate for drift const dReal *rot1 = dBodyGetAngularVel( body ); const dReal *quat_ptr1; dReal quat1[4], quat_len1; quat_ptr1 = dBodyGetQuaternion( body ); quat1[0] = quat_ptr1[0]; quat1[1] = 0; quat1[2] = 0; quat1[3] = quat_ptr1[3]; quat_len1 = sqrt( quat1[0] * quat1[0] + quat1[3] * quat1[3] ); quat1[0] /= quat_len1; quat1[3] /= quat_len1; dBodySetQuaternion( body, quat1 ); dBodySetAngularVel( body, 0, 0, rot1[2] ); dMatrix3 R = { 0, 0, 0, 0, 0, 1}; // 0 0 y if(!rotatebit) //if there is no set rotatebit then don't make rotate in ode { quat1[0] = 0; quat1[1] = 0; quat1[2] = 0; quat1[3] = 1; dBodySetQuaternion( body, quat1 ); dBodySetAngularVel( body, 0, 0, 0 ); } static dVector3 pointrelvel; const dReal *odepos; const dReal *force; dBodyGetRelPointVel(body,0,0,0,pointrelvel); odepos=dBodyGetPosition(body); force = dBodyGetForce (body); dBodySetForce(body,force[0],force[1],0.000); if (odepos[2]>=0.001 || odepos[2]<=-0.001 ){ dBodySetPosition (body,odepos[0],odepos[1], 0.000); dBodyAddRelForce (body,0,0, -(pointrelvel[2])); dBodySetTorque (body, 0.00, 0.00, 0.000); } }
//all keyboard processing void base::active_control(void) { const dReal *odepos; static int wj; static int dj; static int wall_last; if(bitd!=2){ if(keystates['d']){ move(20, 80); last=RIGHT; if(keystates['c']) dBodyAddRelForce(body,-100,0,0); } if(keystates['a']){ move(-20, 80); last=LEFT; if(keystates['z']) dBodyAddRelForce(body,100,0,0); } if(keystates[' ']){ up=check_state(); if (up==0){ dBodyAddForce(body,0.0,40.0,0.0); //if stand wj=0; //can wj dj=0; //can dj wall_last=last; } if ((last==UL1 || last==UR1) && !wj){ wj=1; if(last==UL1 && wall_last!=UL1){ dBodyAddForce(body,10.0,200.0,0.0); //from wall wj=0; } else if(last==UR1 && wall_last!=UR1){ dBodyAddForce(body,-10.0,200.0,0.0); //from wall wj=0; } printf("DJW\n"); wall_last=last; } else if (up==2 && !dj){ // dBodyAddForce(body,0.0,180.0,0.0); //for dj dj=1; } } //--------------------------------- float tmp; up=check_state(); dBodyGetRelPointVel(body,0,0,0,pointrelvel); //get body's speed if(last>1 && !up) tmp=pointrelvel[0]/speed; else if(!up)tmp=0.03; //stand anim speed else tmp=0.4; //jump anim speed if(tmp>=0)calc_speed+=tmp; else calc_speed+=(tmp*-1); if(up){ char *pd = (char*) dGeomGetData (geom); if(last==UP || last==LEFT || last==UL1) if(pd == (char *)'1' && keystates['a'] && pointrelvel[0] > -1 && pointrelvel[0] < 1){ last=UL1; } else last=UL; if(last==DOWN || last==RIGHT || last== UR1) if(pd == (char *)'1' && keystates['d'] && pointrelvel[0] > -1 && pointrelvel[0] < 1){ last=UR1; } else last=UR; } if(!keystates['a'] && !keystates['d'] && !up) //idle // dBodyAddForce(body,-pointrelvel[0]/1.5,0,0); //for active_control _only_! ; if(pointrelvel[0]<=1 && pointrelvel[0]>=-1 && !up) //stand animation if(last!=0 && last!=1) last-=2; if(calc_speed>=1){ translated_val+=(double)1/texture[last].n; calc_speed=0; } if(last_prev!=last) translated_val=0; last_prev=last; dGeomSetData (geom, (void*)'0'); } }