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);
}
예제 #2
0
파일: sound3d.c 프로젝트: AlexSteel/wine
/* 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;
}
예제 #3
0
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;
}
예제 #4
0
파일: sound3d.c 프로젝트: AlexSteel/wine
/* 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;
}
예제 #5
0
파일: Math3d.cpp 프로젝트: junaidnaseer/ivt
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;
}
예제 #7
0
파일: sound3d.c 프로젝트: AlexSteel/wine
/* 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;	
}
예제 #8
0
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);
}