/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
float LSM303_heading()
{
vector_float from = {0,0,1}; //Z axis forward
vector_float temp_m = {LSM303_m.x, LSM303_m.y, LSM303_m.z};
vector_float temp_a = {LSM303_a.x, LSM303_a.y, LSM303_a.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((float)m_min.x + m_max.x) / 2;
temp_m.y -= ((float)m_min.y + m_max.y) / 2;
temp_m.z -= ((float)m_min.z + m_max.z) / 2;
// compute E and N
vector_float E;
vector_float N;
vector_cross(&temp_m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
DEBUGPRINT("Heading: %0.1f\n", heading);
return heading;
}
static int separate_by_axis(vector_s axis, lua_Number hw1, const body_s *body1,
const body_s *body2, vector_s *out)
{
//printf("axiss = %lf, %lf\n", axis.x, axis.y);
lua_Number overlap =
vector_dot(body1->pos, axis) + hw1 +
halfwidth_along_axis(
vector_neg(vector_rotate_from(axis, body2->facing)),
body2->poly) -
vector_dot(body2->pos, axis);
//printf("overlap = %lf - %lf + %lf - %lf = %lf\n",
// vector_dot(body1->pos, axis), hw1,
// halfwidth_along_axis(
// vector_neg(vector_rotate_from(axis, body2->facing)),
// body2->poly),
// vector_dot(body2->pos, axis), overlap);
if(overlap <= 0) return 0;
vector_s correction =
vector_mul(axis, overlap/vector_dot(axis, axis));
if(correction.x == 0 && correction.y == 0) return 0;
//printf("correction = %lf, %lf\n", correction.x, correction.y);
if(vector_dot(correction, correction) < vector_dot(*out, *out))
*out = correction;
return 1;
}
/*
* Calculate light color
*/
color lightColor(const intersection *inter, const ray *srcRay) {
vec3 v = vector_normalized(vector_float_mul(-1.0f, srcRay->dir));
color cD = BLACK, refCoef = reflectCoef(inter->mat.reflect_coef, inter->normal, srcRay->dir);
// For each light calculate and sum color
for (int k = 0; k < num_lights; ++k) {
vec3 l = vector_minus(lights[k].position, inter->position);
float normL = vector_norm(l);
l = vector_normalized(l);
// Look for object between light source and current intersection
ray objectRay; // Ray from object to light
ray_init(&objectRay, inter->position, l, EPSILON, normL, 0);
float interFound = 0.0f;
for (int i = 0; i < object_count; ++i) {
interFound += interDist(&objectRay, &(scene[i]));
}
if (!interFound) {
vec3 h = vector_normalized(vector_add(l, v));
float sc_nl = clamp(vector_dot(inter->normal, l), 0.0f, 1.0f);
float sc_hn = clamp(vector_dot(h, inter->normal), 0.0f, 1.0f);
cD = vector_add(cD,
vector_vec_mul(lights[k].col,
vector_float_mul(sc_nl,
(vector_add(vector_float_mul(1 / M_PI, inter->mat.kd),
(vector_float_mul(pow(sc_hn, inter->mat.shininess), vector_float_mul(((inter->mat.shininess + 8) / (8 * M_PI)), refCoef))))))));
}
}
return cD;
}
bool line_segments_intersect(vector_float2 p1, vector_float2 p2, vector_float2 q1, vector_float2 q2)
{
vector_float2 r = {p2.x - p1.x, p2.y - p1.y};
vector_float2 s = {q2.x - q1.x, q2.y - q1.y};
vector_float2 q_minus_p = {q1.x - p1.x, q1.y - p1.y};
float r_cross_s = vector_cross(r, s).z;
float q_minus_p_cross_r = vector_cross(q_minus_p, r).z;
float q_minus_p_cross_s = vector_cross(q_minus_p, s).z;
if (q_minus_p_cross_r == 0 && r_cross_s == 0) {
// The lines are colinear
float magnitude_r = vector_length(r);
float s_dot_r = vector_dot(s, r);
float t0 = vector_dot(q_minus_p, r) / magnitude_r;
float t1 = t0 + s_dot_r / magnitude_r;
return ((t0 >= 0 && t0 <= 1) || (t1 >= 0 && t1 <= 1));
} else if (r_cross_s == 0 && q_minus_p_cross_r != 0) {
// The lines are parallel and non-intersecting
return false;
} else if (r_cross_s != 0) {
float t = q_minus_p_cross_s / r_cross_s;
float u = q_minus_p_cross_r / r_cross_s;
// Normally you'd want to test for 0 <= t <= 1 && 0 <= u <= 1, but
// that would mean that two lines that share the same endpoint are
// marked as intersecting, which isn't what we want for our use case.
return t > 0 && t < 1 && u > 0 && u < 1;
}
return false;
}
END_TEST
START_TEST(test_vector_three) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
double A[3], B[3], C[3], tmp[3];
double D, E, F, norm;
for (i = 0; i < 3; i++) {
A[i] = mrand / 1e20;
B[i] = mrand / 1e20;
C[i] = mrand / 1e20;
}
/* Check triple product identity */
vector_cross(A, B, tmp);
D = vector_dot(3, C, tmp);
vector_cross(B, C, tmp);
E = vector_dot(3, A, tmp);
vector_cross(C, A, tmp);
F = vector_dot(3, B, tmp);
norm = (vector_norm(3, A) + vector_norm(3, B) + vector_norm(3, C))/3;
fail_unless(fabs(E - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
D, E);
fail_unless(fabs(E - F) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
E, F);
fail_unless(fabs(F - D) < LINALG_TOL * norm,
"Triple product failure between %lf and %lf",
F, D);
}
}
// Calculate the heading
float calcHeading(float *from, const Accelerometer *acc, const Magnetometer *mag)
{
// Change values with values from calibration tests...
int16_t min[] = { 32767, 32767, 32767 };
int16_t max[] = { -32767, -32767, -32767 };
float temp_m[] = { mag->x, mag->y, mag->z };
float temp_a[] = { acc->x, acc->y, acc->z };
// Initialize east and north vector
float east[] = {0, 0, 0};
float north[] = {0, 0, 0};
int i;
for(i = 0; i < 3; i ++)
temp_m[i] -= (min[i]+max[i])/2;
// Calculate North and East vectors
vector_cross(temp_m, temp_a, east);
vector_normalize(east);
vector_cross(temp_a, east, north);
vector_normalize(north);
// Calculate angular difference
float heading = atan2(vector_dot(east, from), vector_dot(north,from))*180/M_PI;
if (heading < 0)
heading += 360;
return heading;
}
END_TEST
START_TEST(test_vector_dot) {
u32 i, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
u32 n = sizerand(MSIZE_MAX);
u32 mid;
if (n % 2 == 0)
mid = n / 2;
else
mid = (n - 1) / 2;
double A[n], B[n];
for (i = 0; i < n; i++) {
A[i] = mrand/1e20;
if (i < mid)
B[n-i-1] = -A[i];
else
B[n-i-1] = A[i];
}
double dot = vector_dot(n, A, B);
if (n % 2 == 0)
fail_unless(fabs(dot) < LINALG_TOL,
"Dot product differs from zero: %lf", vector_dot(n, A, B));
else
fail_unless(fabs(dot - A[mid]*B[mid]) < LINALG_TOL,
"Dot product differs from square of middle element "
"%lf: %lf (%lf)",
A[mid]*B[mid], dot, dot - A[mid]*B[mid]);
}
}
// Returns the angular difference in the horizontal plane between the
// From vector and North, in degrees.
//
// Description of heading algorithm:
// Shift and scale the magnetic reading based on calibration data to
// to find the North vector. Use the acceleration readings to
// determine the Up vector (gravity is measured as an upward
// acceleration). The cross product of North and Up vectors is East.
// The vectors East and North form a basis for the horizontal plane.
// The From vector is projected into the horizontal plane and the
// angle between the projected vector and north is returned.
int LSM303::heading(vector from)
{
// shift and scale
m_dataMag.x = (m_dataMag.x - m_minMag.x) / (m_maxMag.x - m_minMag.x) * 2 - 1.0;
m_dataMag.y = (m_dataMag.y - m_minMag.y) / (m_maxMag.y - m_minMag.y) * 2 - 1.0;
m_dataMag.z = (m_dataMag.z - m_minMag.z) / (m_maxMag.z - m_minMag.z) * 2 - 1.0;
vector temp_a(m_dataAcc);
vector temp_m(m_dataMag);
// normalize
vector_normalize(temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(temp_m, temp_a, E);
vector_normalize(E);
vector_cross(temp_a, E, N);
// compute heading
int heading = round(atan2(vector_dot(E, from), vector_dot(N, from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}
/*
Returns the angular difference in the horizontal plane between the
"from" vector and north, in degrees.
Description of heading algorithm:
Shift and scale the magnetic reading based on calibration data to find
the North vector. Use the acceleration readings to determine the Up
vector (gravity is measured as an upward acceleration). The cross
product of North and Up vectors is East. The vectors East and North
form a basis for the horizontal plane. The From vector is projected
into the horizontal plane and the angle between the projected vector
and horizontal north is returned.
*/
float Compass::getHeading()
{
vector<int> from = (vector<int>){0, -1, 0};
compass->read();
vector<int32_t> temp_m = {compass->magData.x, compass->magData.y, compass->magData.z};
vector<int32_t> temp_a = {compass->accelData.x, compass->accelData.y, compass->accelData.z};
// subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)calMin.x + calMax.x) / 2;
temp_m.y -= ((int32_t)calMin.y + calMax.y) / 2;
temp_m.z -= ((int32_t)calMin.z + calMax.z) / 2;
// compute E and N
vector<float> E;
vector<float> N;
vector_cross(&temp_m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
vector_normalize(&N);
// compute heading
float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * RAD2DEG;
// correcting for mounting direction
heading -= 90.0;
if (heading < 0) heading += 360;
return heading;
}
bool plane_intersect(plane_t p, vector_t ray_start, vector_t ray_direction, vector_t* hit)
{
float denominator = vector_dot(p.normal, ray_direction);
if (denominator == 0)
return false;
float t = vector_dot(p.normal, vector_sub(p.point, ray_start)) / denominator;
if (t < 0.0)
return false;
*hit = vector_add(ray_start, vector_scale(ray_direction, t));
return true;
}
float interDistSphere(const ray *r, const object *obj) {
vec3 ominusc = vector_minus(r->orig, obj->center);
float a = vector_dot(r->dir, r->dir);
float b = 2.0f * vector_dot(r->dir, ominusc);
float c = (vector_dot(ominusc, ominusc) - obj->radius * obj->radius);
float delta = b * b - 4.0f * a * c;
if (delta > 0.0f) {
float sqrtDelta = sqrt(delta);
float t0 = (-b - sqrtDelta) / (2.0f * a), t1 = (-b + sqrtDelta) / (2.0f * a);
return t0 > t1 ? t1 : t0;
}
return -1.0f;
}
int intersection_plan(t_plan *plan, t_ray *ray, double *t)
{
double alpha;
alpha = vector_dot(plan->normal, ray->o) + plan->constante;
alpha = -alpha;
alpha = alpha / vector_dot(plan->normal, ray->d);
if ((alpha > 0.1) && (alpha < *t))
{
*t = alpha;
return (0);
}
return (-1);
}
double MATHNS::angle(std::vector<double> u, std::vector<double> v)
{
double um = vector_mag(u);
double vm = vector_mag(v);
double dot = vector_dot(u,v);
return acos(dot/(um*vm));
}
/**
* sphere_intersect - Check ray intersection with a sphere object.
* @sphere: Pointer to sphere object
* @ray: Pointer to normalized ray object
*
* This function will test if a ray, @ray, will intersect a sphere, @sphere.
* Imaging a ray R with origin at E and direction V intersecting a sphere with
* center O and radius r. The intersection point will be detnoted P. A triangle
* E-O-A, where A is a right angle can be drawn. The E-O side has length c, E-A
* has length v and O-A has length b. See figure 1.
*
* ...---o O ...---o O
* c ...--- | r ...--- |
* ...--- | b ...--- | b
* ...--- | ...--- |
* E o------------------------o A P o------------------------o A
* v d
* fig 1. fig 2.
*
* The v side will then have the same direction as V and i.e. will represent a
* bit of the ray R.
* Pythagorean theorem gives:
* v² + b² = c² (1)
*
* A triangle P-O-A, where A is a right angle can be drawn. The P-O side has
* length r (the sphere radius; remember point P is the sphere intersection
* point), P-A has length d and O-A has length b (same side as previous drawn
* triangle). See figure 2.
*
* Pythagorean theorem gives:
* d² + b² = r² (2)
*
* (1) and (2) gives:
* (1): b² = c² - v²
* (2): d² = r² - b²
* => d² = r² - (c² - v²) (3)
*
* For a vector a and a vector b, the dot product
* a * b = |a| |b| cos(p)
* where |a| and |b| denote the the length of a and b, and p is the angle
* between them. If both a and b have length one (i.e. they are unit vectors),
* their dot product simply gives the cosine of the angle between them. If only
* b is a unit vector, then the dot product a * b gives |a| cos(p), i.e. the
* orthogonal projection of the vector a onto the vector b, with a minus sign
* if the direction is opposite. This is called the scalar projection of a onto
* b. For an intuitive understanding of this formula, recall from trigonometry
* that
* cos(p) = a * b / |a|
* where b is the unit vector
*
* If V is normalized (a unit vector), v is equal to the dot product of E-O
* and V, i.e.
* v = E-O * V
*
* To determine whether an intersection occurs, we compute the value of d. That
* is, if r² - (c² - v²) in (3) is less than 0, d can't be computed and the ray
* does not intersect the sphere. If a intersection occurs, the distance from E
* to the intersection point P is v - d.
*
* Returns:
* Distance from @ray origin to the @sphere intersection point or 0 if no
* intersection was found.
*/
float sphere_intersect (sphere_t* sphere, ray_t* ray)
{
vector_t oe; /* O-E vector in fig 1. */
float c; /* Length of O-E vector, i.e. c in fig 1. */
float v; /* Length of E-A vector, i.e. v in fig 1. */
float d2; /* Computed d² value from formula (3). */
/* Get the direction from the ray origin to the sphere center (O-E) */
vector_sub (&oe, &sphere->center, &ray->origin);
/* Get the length from the ray origin to the sphere center (c) */
c = vector_length (&oe);
/* Get the orthogonal projection of O-E vector onto the V vector,
* i.e. the length of v. */
v = vector_dot (&oe, &ray->dir);
/* If the length of v is less than zero then the ray is going the opposite
* direction and will therefore not intersect the sphere */
if (v < 0 )
return 0.0;
/* Use formula (3) to check for sphere intersection */
d2 = (sphere->radius * sphere->radius) - (c * c) + (v * v);
/* If d2 is less than zero then d can not be computed, i.e. the ray does
* not intersect the sphere */
if (d2 < 0)
return 0.0;
/* The ray hit the sphere, return the distance from the ray origin to
* the intersection point (P) */
return v - sqrt(d2);
}
void LSM303::vector_normalize(vector<float> *a)
{
float mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
void iCompass::vector_normalize(vector<double> *a)
{
double mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
static void compute_dops(const double H[4][4],
const double pos_ecef[3],
dops_t *dops)
{
/* PDOP is the norm of the position elements of tr(H) */
double pdop_sq = H[0][0] + H[1][1] + H[2][2];
dops->pdop = sqrt(pdop_sq);
/* TDOP is like PDOP but for the time state. */
dops->tdop = sqrt(H[3][3]);
/* Calculate the GDOP -- ||tr(H)|| = sqrt(PDOP^2 + TDOP^2) */
dops->gdop = sqrt(pdop_sq + H[3][3]);
/* HDOP and VDOP are Horizontal and Vertical. We could rotate H
* into NED frame and then take the separate components, but a more
* computationally efficient approach is to find the vector in the
* ECEF frame that represents the Down unit vector, and project it
* through H. That gives us VDOP^2, then we find HDOP from the
* relation PDOP^2 = HDOP^2 + VDOP^2. */
double M[3][3];
ecef2ned_matrix(pos_ecef, M);
double down_ecef[4] = {M[2][0], M[2][1], M[2][2], 0};
double tmp[3];
matrix_multiply(3, 4, 1, (double *)H, down_ecef, tmp);
double vdop_sq = vector_dot(3, down_ecef, tmp);
dops->vdop = sqrt(vdop_sq);
dops->hdop = sqrt(pdop_sq - vdop_sq);
}
void vector_normalize(vector_float *a)
{
float mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
void RMG146::vector_normalize(Vector3D *a)
{
float coef = sqrt(vector_dot(a,a));
a->x /= coef;
a->y /= coef;
a->z /= coef;
}
void L3G4200D::vector_normalize(vector *a)
{
float mag = sqrt(vector_dot(a,a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
// Normalize vector
void vector_normalize(float *a)
{
float m = sqrt(vector_dot(a,a));
int i;
for(i = 0; i < 3; i++)
a[i] = a[i]/m;
}
void set_val_plane(t_env *env, double t, t_ray *ray)
{
OBJ.new_start = vector_add(ray->start, vector_scale(t, ray->dir));
OBJ.normal = OBJ.planes[OBJ.cur_plane].rot;
if (vector_dot(ray->dir, OBJ.normal) > 0.0F)
OBJ.normal = vector_sub((t_vector){0.0F, 0.0F, 0.0F}, OBJ.normal);
OBJ.cur_mat = OBJ.mats[OBJ.planes[OBJ.cur_plane].shape.material];
}
void set_val_object(t_env *env, double t, t_ray *ray)
{
OBJ.new_start = vector_add(ray->start, vector_scale(t, ray->dir));
OBJ.normal = OBJ.objects[OBJ.cur_object[0]].faces[OBJ.cur_object[1]].normal;
if (vector_dot(ray->dir, OBJ.normal) > 0.0F)
OBJ.normal = vector_sub((t_vector){0.0F, 0.0F, 0.0F}, OBJ.normal);
OBJ.cur_mat = OBJ.mats[OBJ.objects[OBJ.cur_object[0]].material];
}
float vector_projectPointOnRay(vector2D_t* origin, vector2D_t* direction, vector2D_t* point)
{
vector2D_t c;
c.x = point->x - origin->x;
c.y = point->y - origin->y;
float t = vector_dot(direction, &c);
return t;
}
static void barycentric(vector_t a, vector_t b, vector_t c, vector_t normal, vector_t plane_hit, float* u, float* v, float* w)
{
vector_t ab = vector_sub(b, a);
vector_t ac = vector_sub(c, a);
vector_t ah = vector_sub(plane_hit, a);
float ab_ab = vector_dot(ab, ab);
float ab_ac = vector_dot(ab, ac);
float ac_ac = vector_dot(ac, ac);
float ab_ah = vector_dot(ab, ah);
float ac_ah = vector_dot(ac, ah);
float inv_denom = 1.0 / (ab_ab * ac_ac - ab_ac * ab_ac);
float tmp_v = (ac_ac * ab_ah - ab_ac * ac_ah) * inv_denom;
float tmp_w = (ab_ab * ac_ah - ab_ac * ab_ah) * inv_denom;
*u = 1.0 - tmp_v - tmp_w;
*v = tmp_v;
*w = tmp_w;
}
void set_val_tri(t_env *env, double t, t_ray *ray)
{
OBJ.new_start = vector_add(ray->start, vector_scale(t, ray->dir));
OBJ.normal = TRI[OBJ.cur_tri].normal;
if (vector_dot(ray->dir, OBJ.normal) > 0.0F)
OBJ.normal = vector_sub((t_vector){0.0F, 0.0F, 0.0F}, OBJ.normal);
OBJ.cur_mat = env->obj.mats[TRI[OBJ.cur_tri].shape.material];
}
int get_heading (data_t* data) {
// Initialize heading value - it's an integer because it is
// going to eventually be binned into North, South, East, or West
vector north, east, mag_vector, accel_vector, from;
int heading;
// Initialize north and east vectors
vector_init(&north, 0, 0, 0);
vector_init(&east, 0, 0, 0);
// Put magnetometer and accelerometer data into vector format
vector_init(&mag_vector, data->Mx, data->My, data-> Mz);
vector_init(&accel_vector, data->Ax, data->Ay, data-> Az);
// Initialize "from" vector based on carrier board design
vector_init(&from, 1, 0, 0);
// Subtract calibration offset from magnetometer readings
mag_vector.x -= ((float) MAG_MAX_X + MAG_MIN_X) / 2.0;
mag_vector.y -= ((float) MAG_MAX_Y + MAG_MIN_Y) / 2.0;
mag_vector.z -= ((float) MAG_MAX_Z + MAG_MIN_Z) / 2.0;
// Compute East and North
vector_cross(&mag_vector, &accel_vector, &east);
vector_norm(&east);
vector_cross(&accel_vector, &east, &north);
vector_norm(&north);
// Compute the heading and make sure it's positive
heading = (int)(atan2(vector_dot(&east, &from), vector_dot(&north, &from))* 180 / PI);
if (heading < 0) {
heading += 360;
}
// Memory cleanup
vector_destroy(&north);
vector_destroy(&east);
vector_destroy(&mag_vector);
vector_destroy(&accel_vector);
vector_destroy(&from);
return heading;
}
Vector3D projectPointOnAxis( const Vector3D& point, const Vector3D& axisUnitVector, const Vector3D& axisPoint )
{
Vector3D projection(point - axisPoint);
float projectedLength = vector_dot(projection,axisUnitVector);
projection = projectedLength*axisUnitVector + axisPoint;
return projection;
}
int intersect_ray_plane(t_ray *ray, t_plane *pla, double *t5)
{
double p1;
double p2;
double t;
if ((p2 = vector_dot(ray->dir, pla->rot)) == 0)
return (0);
p1 = vector_dot(pla->shape.pos, pla->rot) -
vector_dot(ray->start, pla->rot);
t = p1 / p2;
if (t > EPSILON)
{
*t5 = t;
return (1);
}
return (0);
}
/*
Tells you if a surface of a polygon is visible or not for drawing.
returns 1 if visible, 0 otherwise
*/
int is_surface_visible(Vector* vpn, Vector* surfaceNormal){
double result = vector_dot(vpn, surfaceNormal);
if( result > 0){
return 1; //visible
}
else{
return 0; // not visible
}
}
