void CircumCenter(Simplex* simplex, double* out)
{
Vertex* a,* b,* c,* d;
GetFaceVerticies(simplex, 0, &a, &b, &c, &d);
double b_a[3] , c_a[3] , d_a[3],
cross1[3], cross2[3], cross3[3],
mult1[3] , mult2[3] , mult3[3],
sum[3];
double denominator;
VertexSub(b->m_Point, a->m_Point, b_a);
VertexSub(c->m_Point, a->m_Point, c_a);
VertexSub(d->m_Point, a->m_Point, d_a);
CrossProduct(b_a, c_a, cross1);
CrossProduct(d_a, b_a, cross2);
CrossProduct(c_a, d_a, cross3);
VertexByScalar(cross1, SquaredDistance(d_a), mult1);
VertexByScalar(cross2, SquaredDistance(c_a), mult2);
VertexByScalar(cross3, SquaredDistance(b_a), mult3);
VertexAdd(mult1, mult2, sum);
VertexAdd(mult3, sum , sum);
denominator = 2*ScalarProduct(b_a, cross3);
VertexByScalar(sum, 1/(double)(denominator), out);
VertexAdd(out, a->m_Point, out);
}
/* magnitude (length) of vector */
static inline D3DVALUE VectorMagnitude (const D3DVECTOR *a)
{
D3DVALUE l;
l = sqrt (ScalarProduct (a, a));
TRACE("|(%f,%f,%f)| = %f\n", a->x, a->y, a->z, l);
return l;
}
bool_t disc_survey( uint8_t center, float radius) {
float wind_dir = atan2(wind_north, wind_east) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (status) {
case UTURN:
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = estimator_x;
c1.y = estimator_y;
float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
c2.x = WaypointX(center) - upwind_x * radius;
c2.y = WaypointY(center) - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
/* calculates the length of vector's projection on another vector */
static inline D3DVALUE ProjectVector (const D3DVECTOR *a, const D3DVECTOR *p)
{
D3DVALUE prod, result;
prod = ScalarProduct(a, p);
result = prod/VectorMagnitude(p);
TRACE("length projection of (%f,%f,%f) on (%f,%f,%f) = %f\n", a->x, a->y, a->z, p->x,
p->y, p->z, result);
return result;
}
void Math3d::MulQuatQuat(const Quaternion &quat1, const Quaternion &quat2, Quaternion &result)
{
Vec3d v;
CrossProduct(quat1.v, quat2.v, v);
v.x += quat1.w * quat2.v.x + quat1.w * quat2.v.x;
v.y += quat1.w * quat2.v.y + quat1.w * quat2.v.y;
v.z += quat1.w * quat2.v.z + quat1.w * quat2.v.z;
result.w = quat1.w * quat2.w - ScalarProduct(quat1.v, quat2.v);
SetVec(result.v, v);
}
double VolumeOfTetrahedron(double* a, double* b, double* c, double* d)
{
double a_d[3], b_d[3], c_d[3], cross[3];
VertexSub(a,d, a_d);
VertexSub(b,d, b_d);
VertexSub(c,d, c_d);
CrossProduct(b_d, c_d, cross);
double point = ScalarProduct(a_d, cross)/(double)6;
return (point >= 0) ? point : -point;
}
/* angle between vectors - rad version */
static inline D3DVALUE AngleBetweenVectorsRad (const D3DVECTOR *a, const D3DVECTOR *b)
{
D3DVALUE la, lb, product, angle, cos;
/* definition of scalar product: a*b = |a|*|b|*cos... therefore: */
product = ScalarProduct (a,b);
la = VectorMagnitude (a);
lb = VectorMagnitude (b);
if (!la || !lb)
return 0;
cos = product/(la*lb);
angle = acos(cos);
TRACE("angle between (%f,%f,%f) and (%f,%f,%f) = %f radians (%f degrees)\n", a->x, a->y, a->z, b->x,
b->y, b->z, angle, RadToDeg(angle));
return angle;
}
bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
if (chemo_sensor) {
last_plume_was_here();
waypoints[plume].x = stateGetPositionEnu_f()->x;
waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
}
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
switch (status) {
case UTURN:
NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
// DownlinkSendWp(c1);
// DownlinkSendWp(c2);
status = CROSSWIND;
nav_init_stage();
}
break;
case CROSSWIND:
NavSegment(c1, c2);
if (NavApproaching(c2, CARROT)) {
waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
if (chemo_sensor) {
waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
}
chemo_sensor = 0;
return TRUE;
}
double SquaredDistance(double* a)
{
return ScalarProduct(a,a);
}
