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