float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){ int i,j; float cand,rdist=0,tdist=0,k=0.0001; //rotation vs translation weight CVector3_t stack[MAX_LINKS]; CVector3_t tar,newrod,rod,diff,pos; CQuat_t q1,q2,q3,q4,rot; // converting data format v_copy(position.GetArray(),pos); q_complete(position.GetArray()+3,rot); q_inv(rot,q1); for(i=0;i<nb_joints;i++){ GetQuaternion(i,q2); q_multiply(q2,q1,q3); q_copy(q3,q1); } for(j=0;j<50;j++){ #ifdef WITH_LAST_LINK last_link->GetTranslation(rod); #else v_clear(rod); #endif InverseKinematicsStack(pos,stack); for(i=nb_joints-1;i>=0;i--){ GetInverseQuaternion(i,q2); q_multiply(q2,q1,q3); //q1*inv(Ri) if(IsInverted(i)==-1){ v_copy(stack[i],tar); Translate(i,rod,newrod); //getting to joint cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k); cand = AngleInClosestRange(i,cand); joints[i]->SetAngle(-cand);// todo to check if it is - Rotate(i,newrod,rod); // updating "rod" v_sub(tar,rod,diff); } else{ Rotate(i,stack[i+1],tar); //rotating back cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k); cand = AngleInClosestRange(i,cand); joints[i]->SetAngle(cand); Rotate(i,rod,newrod); Translate(i,newrod,rod); v_sub(tar,newrod,diff); } GetQuaternion(i,q2); q_multiply(q3,q2,q1); q_multiply(q2,q3,q4); rdist = v_length(q4);//rotation distance, only the first 3 components tdist = v_length(diff);//translation distance // cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl; if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;} } q_multiply(rot,q1,q2); q_inv(rot,q3); q_multiply(q2,q3,q1); } return rdist/rot_tol + tdist; }
double kernel(vector<double> a, vector<double> b) { // double distance; // cout.precision(19); // distance = absDistance(a, b); // distance = pow(distance, 2); // double temp = exp(-sigma * distance); // return temp; return exp(-sigma*(pow(v_length(a),2) + pow(v_length(b),2)- 2 * innerProduct(a,b))); }
/* * returns the length of the variable */ int v_length(var_t *var) { char tmpsb[64]; switch (var->type) { case V_STR: return strlen(var->v.p.ptr); case V_MAP: return map_length(var); case V_PTR: ltostr(var->v.ap.p, tmpsb); return strlen(tmpsb); case V_INT: ltostr(var->v.i, tmpsb); return strlen(tmpsb); case V_NUM: ftostr(var->v.n, tmpsb); return strlen(tmpsb); case V_ARRAY: return var->v.a.size; case V_REF: return v_length(var->v.ref); } return 1; }
//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1] int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){ CMatrix3_t mat; CMatrix3_t invmat; cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2; int i; m_identity(mat); v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not // if(v_squ_length(vec3)<0.00001){ return 0; } v_scale(vec2,-1,v2); m_set_v3_column(vec1,0,mat); m_set_v3_column(v2,1,mat); m_set_v3_column(vec3,2,mat); m_inverse(mat,invmat); v_sub(p2,p1,tmp); v_transform_normal(tmp,invmat,k); for(i=0;i<2;i++){ k[i] = max(min(1,k[i]),0); } v_scale(vec1,k[0],tmp); v_add(p1,tmp,tmp1); v_scale(vec2,k[1],tmp); v_add(p2,tmp,tmp2); v_sub(tmp2,tmp1,tmp); *dist=v_length(tmp); *nearest_point1 = k[0]; *nearest_point2 = k[1]; return 1; }
inline int Reaching::TargetReached(){ cart_vec_t& dist; float d; v_sub(target,pos_cart,dist); d = v_length(dist); return d<tol && HasStopped(); }
double absDistance(vector<double> a, vector<double> b) { vector<double> diff; double temp; for (int i = 0; i < a.size(); i++) { temp = a[i] - b[i]; diff.push_back(temp); } return v_length(diff); }
double pl_area_3d(const Polygon * pl, const int do_holes) { assert(pl); double area= 0.0; if (pl->last < 3) return area; vec tmp; pl_area_vec(pl, tmp); area= v_length(tmp); if (do_holes && pl->holes){ uint i; for (i=0; i < pl->holes->last; i++){ pl_area_vec(pl->holes->polys[i], tmp); area-= v_length(tmp); } } return area; }
void Reaching:: UpdateWeights(){ // -- to modify for visual servo #ifdef BODY_SCHEMA joint_vec_t& min_angle, max_angle; body->GetAnglesLowerBound(min_angle); body->GetAnglesUpperBound(max_angle); #endif #ifdef DYN_WEIGHTS #ifdef SAI_WEIGHT_MODULATION float sai = SpuriousAttractorIndex(); #endif for(int i=0;i<4;i++){ weight_angle[i] = 0.5*base_weight_angle[i]*// (cos((pos_angle[i] - min_angle[i])*2*pi/ (max_angle[i]-min_angle[i])+pi)+1); #ifdef DIST_WEIGHT_MODULATION weight_angle[i] *= (1.0-abs(tar_angle[i]-pos_angle[i])/(max_angle[i]-min_angle[i])); #endif #ifdef SAI_WEIGHT_MODULATION weight_angle[i] *= min(2,sai); #endif } #ifdef END_CART_WEIGHT cart_vec_t& tmp; v_sub(target,pos_cart,tmp); float cart_dist = max(v_length(tmp)/150,1e-10); for(int i=0;i<4;i++){ weight_angle[i] /= cart_dist; } cout<<"dist "<< cart_dist*150<<" weights ";coutvec4(weight_angle); #endif #endif }
//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 v_normalize(vector v){ float len = v_length(v); for(int i=0;i<3;i++) v[i] = v[i]/len; }
void v_normalize(vector result, vector v){ float len = v_length(v); for(int i=0;i<3;i++) result[i] = v[i]/len; result[3]=0; }