Exemplo n.º 1
0
/*
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
/*
 * 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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
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);
  }
}
Exemplo n.º 6
0
// 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;
}
Exemplo n.º 7
0
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]);
  }
}
Exemplo n.º 8
0
// 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;
}
Exemplo n.º 9
0
/*
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;
}
Exemplo n.º 10
0
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;
}
Exemplo n.º 11
0
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;
}
Exemplo n.º 12
0
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);
}
Exemplo n.º 13
0
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));
}
Exemplo n.º 14
0
/**
 * 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);
}
Exemplo n.º 15
0
void LSM303::vector_normalize(vector<float> *a)
{
  float mag = sqrt(vector_dot(a, a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}
Exemplo n.º 16
0
void iCompass::vector_normalize(vector<double> *a)
{
  double mag = sqrt(vector_dot(a, a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}
Exemplo n.º 17
0
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);
}
Exemplo n.º 18
0
void vector_normalize(vector_float *a)
{
	float mag = sqrt(vector_dot(a, a));
	a->x /= mag;
	a->y /= mag;
	a->z /= mag;
}
Exemplo n.º 19
0
void RMG146::vector_normalize(Vector3D *a)
{
	float coef = sqrt(vector_dot(a,a));
	a->x /= coef;
	a->y /= coef;
	a->z /= coef;
}
Exemplo n.º 20
0
void L3G4200D::vector_normalize(vector *a)
{
  float mag = sqrt(vector_dot(a,a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}
Exemplo n.º 21
0
// 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;
}
Exemplo n.º 22
0
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];
}
Exemplo n.º 23
0
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];
}
Exemplo n.º 24
0
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;
}
Exemplo n.º 25
0
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;
}
Exemplo n.º 26
0
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];
}
Exemplo n.º 27
0
 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;


 }
Exemplo n.º 28
0
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;

}
Exemplo n.º 29
0
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);
}
Exemplo n.º 30
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
	}
}