void back_to_destination(const vec3d *v) {
double power = 0.0;
double steering = 0.0;
char msg[100];
vec3d dir;
double dis;
double targetVel;
v3sub(v,&loc,&dir);
dis = v3length(&dir);
if (dis!=0.0) v3normalize(&dir);
targetVel = dis > 15 ? 15 : dis;
if (v3dot(&dir,&front)<0.0) {
steering = 0.3 * v3dot(&dir,&left);
power = -300*(targetVel - v3length(&vel));
power = power > 300 ? 300 : power;
power = power < -300 ? -300 : power;
} else {
steering = 1.0;
power = 150;
}
sprintf(msg,"drive %.16f %.16f",power,steering);
my_send(msg);
}
int check_conflict(const vec3d *src,const vec3d *dest,const vec3d *point,double dis) {
vec3d dir;
double dirLength;
vec3d pDir;
double d;
vec3d dir2;
v3sub(dest,src,&dir);
dirLength = v3length(&dir);
if (dirLength!=0.0) v3scale(&dir,1.0/dirLength,&dir);
v3sub(point,src,&pDir);
d = v3dot(&pDir,&dir);
if (d<0.0)
return 0;
if (d>dirLength)
return 0;
setVec3dToVec3d(&dir,&dir2);
v3simpleRotateY(&dir2,90);
d = fabs(v3dot(&pDir,&dir2));
if (d<dis)
return 1;
else
return 0;
}
void v3crosscross (double * a, double * b, double * c, double * r)
{
double dac, dab;
dac = v3dot(a,c);
dab = v3dot(a,b);
r[0] = dac*b[0] - dab*c[0];
r[1] = dac*b[1] - dab*c[1];
r[2] = dac*b[2] - dab*c[2];
}
V6
fk524(V6 v)
{
v = m6v6(M, v);
/* restore e-terms */
#ifdef ETERMS
{
double m; /* modulus of position vector */
V3 s0;
V3 s1, s1dot;
V3 r0, r0dot;
V3 r1, r1dot;
/* cache the modulus */
m = v6mod(v);
/* convert the state vector back to a unit vector */
v6GetPos(v) = v3scale(v6GetPos(v), 1/m);
v6GetVel(v) = v3scale(v6GetVel(v), CB/(m*as2r(1)));
/* now proceed with the standard treatment */
r1 = v6GetPos(v);
r1dot = v6GetVel(v);
s1 = v3unit(r1);
s0 = s1;
r0 = v3sum(s1, v3diff(A, v3scale(s0, v3dot(s0, A))));
#ifdef ETERMS_ITERATE
s0 = v3unit(r0);
r0 = v3sum(s1, v3diff(A, v3scale(s0, v3dot(s0, A))));
#endif
v6SetPos(v, r0);
#ifdef ETERMS_VEL
s1dot = v3scale(r1dot, 1/v3mod(r1));
r0dot = v3sum(s1dot, v3diff(Adot, v3scale(s0, v3dot(s0, Adot))));
v6SetVel(v, r0dot);
#endif
/* convert the unit vector back into a state vector */
v6GetPos(v) = v3scale(v6GetPos(v), m);
v6GetVel(v) = v3scale(v6GetVel(v), (m*as2r(1))/CB);
}
#endif
return(v);
}
V6
ldeflect(V6 s, V6 e, int flag)
{
double cprime;
double g1;
double g2;
V3 ehat;
V3 p;
V3 qhat;
V3 x; /* the deflection vector */
V3 x1; /* scratch */
V3 x2; /* scratch */
p = v6GetPos(s);
{
V6 v6;
v6 = v6sum(e, s);
qhat = v3unit(v6GetPos(v6));
}
ehat = v3unit(v6GetPos(e));
cprime = 86400.0 * (IAU_C / IAU_AU);
g1 = (2.0 * IAU_K * IAU_K) / (cprime * cprime * v6mod(e));
g2 = 1 + v3dot(qhat, ehat);
/* limit the value of g2 as Patrick Wallace does:
** clip it at 1.0e-5 radians (~922 arcseconds)
*/
if (g2 < 1.0e-5) {
g2 = 1.0e-5;
}
x1 = v3scale(ehat, v3dot(p, qhat));
x2 = v3scale(qhat, v3dot(p, ehat));
x = v3scale(v3diff(x1, x2), g1/g2);
if (flag > 0) {
p = v3sum(p, x);
} else if (flag < 0) {
p = v3diff(p, x);
}
v6SetPos(s, p);
return(s);
}
V6
fk425(V6 v)
{
/* ensure cartesian vectors */
v = v6s2c(v);
#ifdef ETERMS
{
double m; /* modulus of position vector */
V3 u0, u0dot;
V3 u1, u1dot;
/* cache the modulus */
m = v6mod(v);
/* convert the state vector back to a unit vector */
v6GetPos(v) = v3scale(v6GetPos(v), 1/m);
v6GetVel(v) = v3scale(v6GetVel(v), CB/(m*as2r(1)));
/* now proceed with the standard treatment */
u0 = v6GetPos(v);
u1 = v3diff(u0, v3diff(A, v3scale(u0, v3dot(u0, A))));
v6SetPos(v, u1);
#ifdef ETERMS_VEL
u0dot = v6GetVel(v);
u1dot = v3diff(u0dot, v3diff(Adot, v3scale(u0, v3dot(u0, Adot))));
v6SetVel(v, u1dot);
#endif
/* convert the unit vector back into a state vector */
v6GetPos(v) = v3scale(v6GetPos(v), m);
v6GetVel(v) = v3scale(v6GetVel(v), (m*as2r(1))/CB);
}
#endif
v = m6v6(M, v);
return(v);
}
vec4 BTGE_EntBSP_CalcEntityListNodePlane(btWorld wrl,
btEntity lst, vec3 org)
{
btEntity cur;
vec3 dir, tdir;
vec4 norm;
int i, n;
cur=lst; dir=v3zero();
while(cur)
{
tdir=v3sub(btCalcCenter(cur), org);
tdir=v3abs(tdir);
dir=v3add(dir, tdir);
cur=cur->chain;
}
dir=v3norm(dir);
norm=v4plane(dir, v3dot(org, dir));
return(norm);
}
bool IMU_GetErectorQuat( quat *q_erect_out, quat *frame, vec3 *v_measured, vec3 *v_reference, float rads_sec, float dt)
// Calculates the quaternion needed to rotate the measured vector to the reference vector (world space) at a fixed correction rate
// Returns TRUE if correction is necessary, or FALSE if it is below the threshold of caring.
{
// Get the rotation axis we'll use to erect.
// (Normalize returns the length. We do a lower limit. No sense in correcting if we're close)
vec3 c;
v3cross(&c,v_measured, v_reference);
if( v3normalize(&c,&c) > 1.0f * RADIANS_PER_DEGREE)
{
// Get the angle between the two vectors, and clamp to that limit. We don't want to overshoot.
// Angles are always positive since the rotation angle flips appropriately.
float rads = rads_sec * g_rps_scale * dt;
float a = fabs(clamped_acos(v3dot(v_measured, v_reference)));
if((rads>a) && (rads>0))
{
rads=a;
}
else
if((rads<-a) && (rads<0))
{
rads=-a;
}
//ULIMIT(rads,a);
// Get the quat that rotates our sensor toward the world vector by the specified amount
Quat_SetAxisAndAngle( q_erect_out, &c, rads);
return true;
}
return false;
}