// Möller–Trumbore intersection algorithm
bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
{
  const double EPSILON = 0.000001;

  cv::Point3f e1, e2;
  cv::Point3f P, Q, T;
  double det, inv_det, u, v;
  double t;

  cv::Point3f V1 = Triangle.getV0();  // Triangle vertices
  cv::Point3f V2 = Triangle.getV1();
  cv::Point3f V3 = Triangle.getV2();

  cv::Point3f O = Ray.getP0(); // Ray origin
  cv::Point3f D = Ray.getP1(); // Ray direction

  //Find vectors for two edges sharing V1
  e1 = SUB(V2, V1);
  e2 = SUB(V3, V1);

  // Begin calculation determinant - also used to calculate U parameter
  P = CROSS(D, e2);

  // If determinant is near zero, ray lie in plane of triangle
  det = DOT(e1, P);

  //NOT CULLING
  if(det > -EPSILON && det < EPSILON) return false;
  inv_det = 1.f / det;

  //calculate distance from V1 to ray origin
  T = SUB(O, V1);

  //Calculate u parameter and test bound
  u = DOT(T, P) * inv_det;

  //The intersection lies outside of the triangle
  if(u < 0.f || u > 1.f) return false;

  //Prepare to test v parameter
  Q = CROSS(T, e1);

  //Calculate V parameter and test bound
  v = DOT(D, Q) * inv_det;

  //The intersection lies outside of the triangle
  if(v < 0.f || u + v  > 1.f) return false;

  t = DOT(e2, Q) * inv_det;

  if(t > EPSILON) { //ray intersection
    *out = t;
    return true;
  }

  // No hit, no win
  return false;
}

static void
findstats(Pos p, Ori o)
{
	/* Recalculate cross assert and score total at 'p'
	 */
	Pos	left, right;
	Word lword, rword;
	Node n;
	Edge e;
	int	s;

	lword.n = rword.n = 0;
	if(EDGE(p))
		return;

	/* find word to the left */
	s = 0;
	for(left=PREV(p,o); HASLETTER(left); left = PREV(left,o))
		;
	left = NEXT(left,o);
	while (HASLETTER(left)) {
		lword.c[lword.n++] = LETTER(left);
		s += SCORE(left);
		left = NEXT(left,o);
	}
	/* find word to the right */
	for(right=NEXT(p,o); HASLETTER(right); right = NEXT(right,o)) {
		rword.c[rword.n++] = LETTER(right);
		s += SCORE(right);
	}
	if(DBG) {
		wordprint(&lword);
		print("X");
		wordprint(&rword);
		print(" [%d] ", s);
	}

	SIDE(p,o) = s;
	ISANCHOR(p) = true;

	/* calculate cross asserts */
	CROSS(p,o) = 0;
	n = traverse(root, &lword, 0);
	assert(n>=0);
	if(n>0)
		do {
			e = dict[n++];
			if ( (rword.n && isword(NODE(e), &rword)) || 
				 (!rword.n && TERM(e)) ) {
				CROSS(p,o) |= 1 << LET(e);
				DPRINT("%c, ", LET(e)+'a');
			}
		} while (!(LAST(e)));
	DPRINT("\n");
}

/* and one CROSS has been moved out from the if-else if-else */
int intersect_triangle3(double orig[3], double dir[3],
			double vert0[3], double vert1[3], double vert2[3],
			double *t, double *u, double *v)
{
   double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
   double det,inv_det;

   /* find vectors for two edges sharing vert0 */
   SUB(edge1, vert1, vert0);
   SUB(edge2, vert2, vert0);

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);
   inv_det = 1.0 / det;
   
   CROSS(qvec, tvec, edge1);
      
   if (det > EPSILON)
   {
      *u = DOT(tvec, pvec);
      if (*u < 0.0 || *u > det)
	 return 0;
            
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec);
      if (*v < 0.0 || *u + *v > det)
	 return 0;
      
   }
   else if(det < -EPSILON)
   {
      /* calculate U parameter and test bounds */
      *u = DOT(tvec, pvec);
      if (*u > 0.0 || *u < det)
	 return 0;
      
      /* calculate V parameter and test bounds */
      *v = DOT(dir, qvec) ;
      if (*v > 0.0 || *u + *v < det)
	 return 0;
   }
   else return 0;  /* ray is parallell to the plane of the triangle */

   *t = DOT(edge2, qvec) * inv_det;
   (*u) *= inv_det;
   (*v) *= inv_det;

   return 1;
}

/* and one CROSS has been moved out from the if-else if-else */
int intersect_triangle3(const RAYTRI *rt)
{
	double u, v;
	double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
	double det,inv_det;

	/* find vectors for two edges sharing rt->v0 */
	SUB(edge1, rt->v1, rt->v0);
	SUB(edge2, rt->v2, rt->v0);

	/* begin calculating determinant - also used to calculate U parameter */
	CROSS(pvec, rt->dir, edge2);

	/* if determinant is near zero, ray lies in plane of triangle */
	det = DOT(edge1, pvec);

	/* calculate distance from rt->v0 to ray origin */
	SUB(tvec, rt->org, rt->v0);
	inv_det = 1.0 / det;

	CROSS(qvec, tvec, edge1);

	if (det > EPSILON)
	{
		u = DOT(tvec, pvec);
		if (u < 0.0 || u > det)
			return 0;

		/* calculate V parameter and test bounds */
		v = DOT(rt->dir, qvec);
		if (v < 0.0 || u + v > det)
			return 0;

	}
	else if(det < -EPSILON)
	{
		/* calculate U parameter and test bounds */
		u = DOT(tvec, pvec);
		if (u > 0.0 || u < det)
			return 0;

		/* calculate V parameter and test bounds */
		v = DOT(rt->dir, qvec) ;
		if (v > 0.0 || u + v < det)
			return 0;
	}
	else return 0;  /* ray is parallell to the plane of the triangle */

	double t = DOT(edge2, qvec) * inv_det;
	if(t<0.0 || t>1.0)	return 0;

	return 1;
}

long point_triangle_intersection(Point3 p, Triangle3 t)
{
long sign12,sign23,sign31;
Point3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
Point3 cross12_1p,cross23_2p,cross31_3p;

/* First, a quick bounding-box test:                               */
/* If P is outside triangle bbox, there cannot be an intersection. */

   if (p.x > MAX3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);  
   if (p.y > MAX3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z > MAX3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);
   if (p.x < MIN3(t.v1.x, t.v2.x, t.v3.x)) return(OUTSIDE);
   if (p.y < MIN3(t.v1.y, t.v2.y, t.v3.y)) return(OUTSIDE);
   if (p.z < MIN3(t.v1.z, t.v2.z, t.v3.z)) return(OUTSIDE);

/* For each triangle side, make a vector out of it by subtracting vertexes; */
/* make another vector from one vertex to point P.                          */
/* The crossproduct of these two vectors is orthogonal to both and the      */
/* signs of its X,Y,Z components indicate whether P was to the inside or    */
/* to the outside of this triangle side.                                    */

   SUB(t.v1, t.v2, vect12)
   SUB(t.v1,    p, vect1h);
   CROSS(vect12, vect1h, cross12_1p)
   sign12 = SIGN3(cross12_1p);      /* Extract X,Y,Z signs as 0..7 or 0...63 integer */

   SUB(t.v2, t.v3, vect23)
   SUB(t.v2,    p, vect2h);
   CROSS(vect23, vect2h, cross23_2p)
   sign23 = SIGN3(cross23_2p);

   SUB(t.v3, t.v1, vect31)
   SUB(t.v3,    p, vect3h);
   CROSS(vect31, vect3h, cross31_3p)
   sign31 = SIGN3(cross31_3p);

/* If all three crossproduct vectors agree in their component signs,  */
/* then the point must be inside all three.                           */
/* P cannot be OUTSIDE all three sides simultaneously.                */

   /* this is the old test; with the revised SIGN3() macro, the test
    * needs to be revised. */
#ifdef OLD_TEST
   if ((sign12 == sign23) && (sign23 == sign31))
      return(INSIDE);
   else
      return(OUTSIDE);
#else
   return ((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE;
#endif
}

void Camera::setFaceToOrigin() {
    //    logger.prs(theta);
    //    logger.prl(gamma);

    double rad = position.mag();

    position.x = rad * cos(D2R(theta)) * cos(D2R(gamma));
    position.y = rad * sin(D2R(theta)) * cos(D2R(gamma));
    position.z = rad * sin(D2R(gamma));

    forward = -position;
    up = CROSS(CROSS(forward, Vector(0, 0, 1)), forward);
}

hacd::HaI32 tri_tri_overlap_test_3d(hacd::HaF32 p1[3], hacd::HaF32 q1[3], hacd::HaF32 r1[3], 

			    hacd::HaF32 p2[3], hacd::HaF32 q2[3], hacd::HaF32 r2[3])
{
  hacd::HaF32 dp1, dq1, dr1, dp2, dq2, dr2;
  hacd::HaF32 v1[3], v2[3];
  hacd::HaF32 N1[3], N2[3]; 
  
  /* Compute distance signs  of p1, q1 and r1 to the plane of
     triangle(p2,q2,r2) */


  SUB(v1,p2,r2)
  SUB(v2,q2,r2)
  CROSS(N2,v1,v2)

  SUB(v1,p1,r2)
  dp1 = DOT(v1,N2);
  SUB(v1,q1,r2)
  dq1 = DOT(v1,N2);
  SUB(v1,r1,r2)
  dr1 = DOT(v1,N2);
  
  if (((dp1 * dq1) > 0.0f) && ((dp1 * dr1) > 0.0f))  return 0; 

  /* Compute distance signs  of p2, q2 and r2 to the plane of
     triangle(p1,q1,r1) */

  
  SUB(v1,q1,p1)
  SUB(v2,r1,p1)
  CROSS(N1,v1,v2)

  SUB(v1,p2,r1)
  dp2 = DOT(v1,N1);
  SUB(v1,q2,r1)
  dq2 = DOT(v1,N1);
  SUB(v1,r2,r1)
  dr2 = DOT(v1,N1);
  
  if (((dp2 * dq2) > 0.0f) && ((dp2 * dr2) > 0.0f)) return 0;

  /* Permutation in a canonical form of T1's vertices */


  if (dp1 > 0.0f) {
    if (dq1 > 0.0f) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
    else if (dr1 > 0.0f) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)	
    else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
  } else if (dp1 < 0.0f) {

void PenaltyMarkPercept::draw() const
{
  DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:image", "drawingOnImage");
  DECLARE_DEBUG_DRAWING("representation:PenaltyMarkPercept:field", "drawingOnField");

  if(Blackboard::getInstance().exists("FrameInfo"))
  {
    const FrameInfo& frameInfo = static_cast<const FrameInfo&>(Blackboard::getInstance()["FrameInfo"]);
    if(timeLastSeen == frameInfo.time)
    {
      CROSS("representation:PenaltyMarkPercept:image", position.x(), position.y(), 5, 5, Drawings::solidPen, ColorRGBA::blue);
      CROSS("representation:PenaltyMarkPercept:field", positionOnField.x(), positionOnField.y(), 40, 40, Drawings::solidPen, ColorRGBA::blue);
    }
  }
}

int
main(int argc,char *argv[])
{
	/* rotates input vector by specified angle around given rotation axis */

	VECTOR r,n,nxr,rp;
	double phi,cphi,sphi,ndotr;

	if (argc != 8) {
		(void) fprintf(stderr,"Usage: %s x y z phi rx ry rz\n",argv[0]);
		return 1;
		}

	SET_VEC(r,atof(argv[1]),atof(argv[2]),atof(argv[3]));
	assert(MAG(r) > 0.0);
	phi = atof(argv[4]);
	SET_VEC(n,atof(argv[5]),atof(argv[6]),atof(argv[7]));
	assert(MAG(n) > 0.0);

	ndotr = DOT(n,r);
	CROSS(n,r,nxr);
	cphi = cos(phi);
	sphi = sin(phi);

	SCALE_VEC(r,cphi);
	SCALE_VEC(n,ndotr*(1.0-cphi));
	SCALE_VEC(nxr,sphi);

	ADD_VEC(r,n,rp);
	ADD_VEC(rp,nxr,rp);

	(void) printf("%g %g %g\n",rp[X],rp[Y],rp[Z]);

	return 0;
	}

/**
 * @name angle_change
 *
 * Return the change in angle (degrees) of the line segments between
 * points one and two, and two and three.
 */
int Wordrec::angle_change(EDGEPT *point1, EDGEPT *point2, EDGEPT *point3) {
  VECTOR vector1;
  VECTOR vector2;

  int angle;

  /* Compute angle */
  vector1.x = point2->pos.x - point1->pos.x;
  vector1.y = point2->pos.y - point1->pos.y;
  vector2.x = point3->pos.x - point2->pos.x;
  vector2.y = point3->pos.y - point2->pos.y;
  /* Use cross product */
  float length = std::sqrt(static_cast<float>(LENGTH(vector1)) * LENGTH(vector2));
  if (static_cast<int>(length) == 0)
    return (0);
  angle = static_cast<int>(floor(asin(CROSS (vector1, vector2) /
                                      length) / M_PI * 180.0 + 0.5));

  /* Use dot product */
  if (SCALAR (vector1, vector2) < 0)
    angle = 180 - angle;
  /* Adjust angle */
  if (angle > 180)
    angle -= 360;
  if (angle <= -180)
    angle += 360;
  return (angle);
}

void MotionRequest::draw() const
{
  DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
  if(motion == walk)
  {
    switch(walkRequest.mode)
    {
    case WalkRequest::targetMode:
    {
      LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x, walkRequest.target.translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
      CROSS("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, 50, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
      Vector2<> rotation(500.f, 0.f);
      rotation.rotate(walkRequest.target.rotation);
      ARROW("representation:MotionRequest", walkRequest.target.translation.x, walkRequest.target.translation.y, walkRequest.target.translation.x + rotation.x, walkRequest.target.translation.y + rotation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
    }
    break;
    case WalkRequest::speedMode:
    case WalkRequest::percentageSpeedMode:
    {
      Vector2<> translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f;
      ARROW("representation:MotionRequest", 0, 0, translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0));
      if(walkRequest.target.rotation != 0.0f)
      {
        translation.x = translation.abs();
        translation.y = 0;
        translation.rotate(walkRequest.speed.rotation);
        ARROW("representation:MotionRequest", 0, 0,  translation.x, translation.y, 0, Drawings::ps_solid, ColorRGBA(0xcd, 0, 0, 127));
      }
    }
    break;
    default:
      break;
    }
  }
}

void AseFile::ComputeNormals(zASE_Object &obj)
{
	if(obj.pFaceNormals==NULL && obj.numOfFaces>0)obj.pFaceNormals = new vec[obj.numOfFaces];
	if(obj.pFaceNormals==NULL)return;
	if(obj.pNormals==NULL && obj.numOfVerts>0)obj.pNormals = new vec[obj.numOfVerts];
	if(obj.pNormals==NULL)return;
	
	for(int j=0; j<obj.numOfFaces; j++)
	{
		vec a,b;
		a = obj.pVerts[obj.pFaces[j].index[1]] - obj.pVerts[obj.pFaces[j].index[0]];
		b = obj.pVerts[obj.pFaces[j].index[2]] - obj.pVerts[obj.pFaces[j].index[0]];
		obj.pFaceNormals[j] = CROSS( a, b);
		obj.pFaceNormals[j].Normalize();
	}

	int count;

	for( j=0; j<obj.numOfVerts; j++)
	{
		count=0;
		obj.pNormals[j].clear();
		for(int k=0; k<obj.numOfFaces; k++)
		{
			if( obj.pFaces[k].index[0]==j || obj.pFaces[k].index[1]==j ||	obj.pFaces[k].index[2]==j )
			{
				obj.pNormals[j] += obj.pFaceNormals[k];
				count++;
			}
		}
		obj.pNormals[j] *= 1.f/(float)count;
		obj.pNormals[j].Normalize();
	}
}

///
//	PointTriangleIntersection()
//
//		Test if 3D point is inside 3D triangle 
static
int PointTriangleIntersection(const vector3& p, const TRI& t)
{
	int		sign12,sign23,sign31;
	vector3 vect12,vect23,vect31,vect1h,vect2h,vect3h;
	vector3 cross12_1p,cross23_2p,cross31_3p;

	///
	//	First, a quick bounding-box test:                               
	//  If P is outside triangle bbox, there cannot be an intersection. 
	//
	if (p.x() > MAX3(t.m_P[0].x(), t.m_P[1].x(), t.m_P[2].x())) return(OUTSIDE);  
	if (p.y() > MAX3(t.m_P[0].y(), t.m_P[1].y(), t.m_P[2].y())) return(OUTSIDE);
	if (p.z() > MAX3(t.m_P[0].z(), t.m_P[1].z(), t.m_P[2].z())) return(OUTSIDE);
	if (p.x() < MIN3(t.m_P[0].x(), t.m_P[1].x(), t.m_P[2].x())) return(OUTSIDE);
	if (p.y() < MIN3(t.m_P[0].y(), t.m_P[1].y(), t.m_P[2].y())) return(OUTSIDE);
	if (p.z() < MIN3(t.m_P[0].z(), t.m_P[1].z(), t.m_P[2].z())) return(OUTSIDE);

	///
	//	For each triangle side, make a vector out of it by subtracting vertexes; 
	//	make another vector from one vertex to point P.                          
	//  The crossproduct of these two vectors is orthogonal to both and the      
	//  signs of its X,Y,Z components indicate whether P was to the inside or    
	//  to the outside of this triangle side.                                    
	//
	SUB(t.m_P[0], t.m_P[1], vect12);
	SUB(t.m_P[0], p,		  vect1h);	
	CROSS(vect12, vect1h, cross12_1p)
	sign12 = SIGN3(cross12_1p);      /* Extract X,Y,Z signs as 0..7 or 0...63 integer */

	SUB(t.m_P[1], t.m_P[2], vect23)
	SUB(t.m_P[1],    p, vect2h);
	CROSS(vect23, vect2h, cross23_2p)
	sign23 = SIGN3(cross23_2p);

	SUB(t.m_P[2], t.m_P[0], vect31)
	SUB(t.m_P[2],    p, vect3h);
	CROSS(vect31, vect3h, cross31_3p)
	sign31 = SIGN3(cross31_3p);

	///
	//	If all three crossproduct vectors agree in their component signs, /
	//  then the point must be inside all three.                           
	//  P cannot be OUTSIDE all three sides simultaneously.                
	//
	return (((sign12 & sign23 & sign31) == 0) ? OUTSIDE : INSIDE);
}

int
intersect_triangle(const float orig[3], const float dir[3],
                   const float vert0[3], const float vert1[3], const float vert2[3],
                   float *t, float *u, float *v)
{
   float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
   float det,inv_det;

   /* find vectors for two edges sharing vert0 */
   SUB(edge1, vert1, vert0);
   SUB(edge2, vert2, vert0);

   /* begin calculating determinant - also used to calculate U parameter */
   CROSS(pvec, dir, edge2);

   /* if determinant is near zero, ray lies in plane of triangle */
   det = DOT(edge1, pvec);

   if (det > -EPSILON && det < EPSILON)
     return 0;
   inv_det = 1.0f / det;

   /* calculate distance from vert0 to ray origin */
   SUB(tvec, orig, vert0);

   /* calculate U parameter and test bounds */
   *u = DOT(tvec, pvec) * inv_det;
   if (*u < 0.0 || *u > 1.0)
     return 0;

   /* prepare to test V parameter */
   CROSS(qvec, tvec, edge1);

   /* calculate V parameter and test bounds */
   *v = DOT(dir, qvec) * inv_det;
   if (*v < 0.0 || *u + *v > 1.0)
     return 0;

   /* calculate t, ray intersects triangle */
   *t = DOT(edge2, qvec) * inv_det;

   return 1;
}

void
bp_camera_set_look_at (camera_t *camera, vector_t look_at)
{
	float dir_length, up_length, right_length;
	vector_t dir;     /* direction vector not normalized */

	dir_length   = MAG (camera->direction);
	up_length    = MAG (camera->up);
	right_length = MAG (camera->right);

	SUB   (dir, look_at, camera->location);

	CROSS   (camera->right, dir, camera->sky);
	VRESIZE (camera->right, right_length);

	CROSS   (camera->up, camera->right, dir);
	VRESIZE (camera->up, up_length);

	VSET_SIZE (camera->direction, dir, dir_length);
}

static void tri_normal(tri * trn, vector  * pnt, ray * incident, vector * N) {

  CROSS((*N), trn->edge1, trn->edge2);

  VNorm(N);

  if (VDot(N, &(incident->d)) > 0.0)  {
    N->x=-N->x;
    N->y=-N->y;
    N->z=-N->z;
  }
}

// Computes the min and max cross product of the outline points with the
// given vec and returns the results in min_xp and max_xp. Geometrically
// this is the left and right edge of the outline perpendicular to the
// given direction, but to get the distance units correct, you would
// have to divide by the modulus of vec.
void TESSLINE::MinMaxCrossProduct(const TPOINT vec,
                                  int* min_xp, int* max_xp) const {
  *min_xp = MAX_INT32;
  *max_xp = MIN_INT32;
  EDGEPT* this_edge = loop;
  do {
    if (!this_edge->IsHidden() || !this_edge->prev->IsHidden()) {
      int product = CROSS(this_edge->pos, vec);
      UpdateRange(product, min_xp, max_xp);
    }
    this_edge = this_edge->next;
  } while (this_edge != loop);
}

void shNgin3dMove(float frontSpeed, float rightSpeed, float upSpeed, unsigned long dt)
{
	// cashed version of conversion of latitude and longitude to points already exist in _Ngin3d_cameraTarget and _Ngin3d_cameraUp
	float targetDir[3] = {_Ngin3d_cameraTarget[0]-_Ngin3d_cameraPosition[0], _Ngin3d_cameraTarget[1]-_Ngin3d_cameraPosition[1], _Ngin3d_cameraTarget[2]-_Ngin3d_cameraPosition[2]};
	float rightDir[3] = {CROSS(targetDir, _Ngin3d_cameraUp)};		// targetDir and _Ngin3d_cameraUp should already be normalized
	float upDir[3] = {0, 1, 0};
	if (g_Ngin3d_uprightMode)
	{
		if (_Ngin3d_cameraUp[1] < 0)
			upDir[1] = -1;
	}
	else
	{
		upDir[0] = _Ngin3d_cameraUp[0];
		upDir[1] = _Ngin3d_cameraUp[1];
		upDir[2] = _Ngin3d_cameraUp[2];
	}
	float speed[3] = {frontSpeed*targetDir[0]+upSpeed*upDir[0]+rightSpeed*rightDir[0],
			frontSpeed*targetDir[1]+upSpeed*upDir[1]+rightSpeed*rightDir[1],
			frontSpeed*targetDir[2]+upSpeed*upDir[2]+rightSpeed*rightDir[2]};
	float gravity[3] = {g_Ngin3d_settings[NGIN3D_GRAVITY_X], g_Ngin3d_settings[NGIN3D_GRAVITY_Y], g_Ngin3d_settings[NGIN3D_GRAVITY_Z]};
	float normGravity = NORM(gravity);
	if (g_Ngin3d_uprightMode && !STATE_IN_AIR && normGravity > EPSILON)
	{
		float speedInDirectionOfGravity = DOT(speed, gravity)/normGravity/normGravity;
		speed[0] -= gravity[0]*speedInDirectionOfGravity;
		speed[1] -= gravity[1]*speedInDirectionOfGravity;
		speed[2] -= gravity[2]*speedInDirectionOfGravity;
	}
	shNgin3dCollisionResult cr = g_Ngin3d_player.shNgin3dPOUpdate(dt, speed);
	if (cr == NGIN3D_COLLISION_WITH_GROUND)
	{
		STATE_IN_AIR = false;
	}
	float cameraDisplacement[3] = {0, 0, 0};
	if (g_Ngin3d_uprightMode)
	{
		if (STATE_CROUCHED)
			cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_CROUCHED_HEIGHT];
		else
			cameraDisplacement[1] = g_Ngin3d_settings[NGIN3D_HEIGHT];
	}
	_Ngin3d_cameraPosition[0] = g_Ngin3d_player.position[0]+cameraDisplacement[0];
	_Ngin3d_cameraPosition[1] = g_Ngin3d_player.position[1]+cameraDisplacement[1];
	_Ngin3d_cameraPosition[2] = g_Ngin3d_player.position[2]+cameraDisplacement[2];
	_Ngin3d_cameraTarget[0] = _Ngin3d_cameraPosition[0]+targetDir[0];
	_Ngin3d_cameraTarget[1] = _Ngin3d_cameraPosition[1]+targetDir[1];
	_Ngin3d_cameraTarget[2] = _Ngin3d_cameraPosition[2]+targetDir[2];
	(*_Ngin3d_cameraPositionFunction)(_Ngin3d_cameraPosition);
	(*_Ngin3d_cameraTargetFunction)(_Ngin3d_cameraTarget);
}

/**********************************************************************
 * divide_blobs
 *
 * Create two blobs by grouping the outlines in the appropriate blob.
 * The outlines that are beyond the location point are moved to the
 * other blob.  The ones whose x location is less than that point are
 * retained in the original blob.
 **********************************************************************/
void divide_blobs(TBLOB *blob, TBLOB *other_blob, bool italic_blob,
                  const TPOINT& location) {
  TPOINT vertical = italic_blob ? kDivisibleVerticalItalic
                                : kDivisibleVerticalUpright;
  TESSLINE *outline1 = NULL;
  TESSLINE *outline2 = NULL;

  TESSLINE *outline = blob->outlines;
  blob->outlines = NULL;
  int location_prod = CROSS(location, vertical);

  while (outline != NULL) {
    TPOINT mid_pt = {(outline->topleft.x + outline->botright.x) / 2,
                     (outline->topleft.y + outline->botright.y) / 2};
    int mid_prod = CROSS(mid_pt, vertical);
    if (mid_prod < location_prod) {
      // Outline is in left blob.
      if (outline1)
        outline1->next = outline;
      else
        blob->outlines = outline;
      outline1 = outline;
    } else {
      // Outline is in right blob.
      if (outline2)
        outline2->next = outline;
      else
        other_blob->outlines = outline;
      outline2 = outline;
    }
    outline = outline->next;
  }

  if (outline1)
    outline1->next = NULL;
  if (outline2)
    outline2->next = NULL;
}

/**********************************************************************
 * divisible_blob
 *
 * Returns true if the blob contains multiple outlines than can be
 * separated using divide_blobs. Sets the location to be used in the
 * call to divide_blobs.
 **********************************************************************/
bool divisible_blob(TBLOB *blob, bool italic_blob, TPOINT* location) {
  if (blob->outlines == NULL || blob->outlines->next == NULL)
    return false;  // Need at least 2 outlines for it to be possible.
  int max_gap = 0;
  TPOINT vertical = italic_blob ? kDivisibleVerticalItalic
                                : kDivisibleVerticalUpright;
  for (TESSLINE* outline1 = blob->outlines; outline1 != NULL;
       outline1 = outline1->next) {
    if (outline1->is_hole)
      continue;  // Holes do not count as separable.
    TPOINT mid_pt1 = {(outline1->topleft.x + outline1->botright.x) / 2,
                      (outline1->topleft.y + outline1->botright.y) / 2};
    int mid_prod1 = CROSS(mid_pt1, vertical);
    int min_prod1, max_prod1;
    outline1->MinMaxCrossProduct(vertical, &min_prod1, &max_prod1);
    for (TESSLINE* outline2 = outline1->next; outline2 != NULL;
         outline2 = outline2->next) {
      if (outline2->is_hole)
        continue;  // Holes do not count as separable.
      TPOINT mid_pt2 = {  (outline2->topleft.x + outline2->botright.x) / 2,
                        (outline2->topleft.y + outline2->botright.y) / 2};
      int mid_prod2 = CROSS(mid_pt2, vertical);
      int min_prod2, max_prod2;
      outline2->MinMaxCrossProduct(vertical, &min_prod2, &max_prod2);
      int mid_gap = abs(mid_prod2 - mid_prod1);
      int overlap = MIN(max_prod1, max_prod2) - MAX(min_prod1, min_prod2);
      if (mid_gap - overlap / 2 > max_gap) {
        max_gap = mid_gap - overlap / 2;
        *location = mid_pt1;
        *location += mid_pt2;
        *location /= 2;
      }
    }
  }
  // Use the y component of the vertical vector as an approximation to its
  // length.
  return max_gap > vertical.y;
}