C++ CROSS函数代码示例

// 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);
}