void DrawArrow( float px, float py, float pz,
    float nx, float ny, float nz,
    double arrowEndWidth, double arrowEndLength )
{
  GLdouble normal[3], cross[3], zaxis[3];
  GLdouble angle;

  #define DegreesToRadians     (3.14159265 / (GLfloat) 180.0)

  normal[0] = nx; normal[1] = ny; normal[2] = nz;
  double len = sqrt(nx*nx + ny*ny + nz*nz);

  if (len < 1E-6)
	return;


  glPushMatrix();
    glTranslatef( px, py, pz );
    glBegin( GL_LINES );
      glVertex3f( 0.0, 0.0, 0.0 );
      glVertex3f( nx, ny, nz );
    glEnd();

    // normalize the normal vector 
	normal[0] /= len;
	normal[1] /= len;
	normal[2] /= len;

    // determine angle between z axis and normal 
    zaxis[0] = 0; zaxis[1] = 0; zaxis[2] = 1;
    angle = acos( zaxis[0]*normal[0] + zaxis[1]*normal[1] + zaxis[2]*normal[2] )/DegreesToRadians;
         
    if ( angle != 0.0 ) 
	{
      // find the axis of rotation 
      CROSSPRODUCT( zaxis[0], zaxis[1], zaxis[2], 
		            normal[0], normal[1], normal[2], 
					cross[0], cross[1], cross[2] );
      glRotatef( angle, cross[0], cross[1], cross[2] );
    }

    // move to end of normal vector 
    glTranslatef( 0.0, 0.0, len );
	/*
	glScalef(0.3,0.3,0.3);
    #ifdef EIFFEL140VOX
  	  glScalef(2.0,2.0,2.0);
    #endif
    #ifdef SPOON100VOX
  	  glScalef(2.0,2.0,2.0);
    #endif
	*/
    glutSolidCone( len*arrowEndWidth, len*arrowEndLength, 12, 1 );
  glPopMatrix();
}
Beispiel #2
0
/*>REAL blPointLineDistance(REAL Px, REAL Py, REAL Pz,
                          REAL P1x, REAL P1y, REAL P1z,
                          REAL P2x, REAL P2y, REAL P2z,
                          REAL *Rx, REAL *Ry, REAL *Rz,
                          REAL *frac)
   ------------------------------------------------------
*//**
   \param[in]     Px          Point x coordinate
   \param[in]     Py          Point y coordinate
   \param[in]     Pz          Point z coordinate
   \param[in]     P1x         Line start x coordinate
   \param[in]     P1y         Line start y coordinate
   \param[in]     P1z         Line start z coordinate
   \param[in]     P2x         Line end x coordinate
   \param[in]     P2y         Line end y coordinate
   \param[in]     P2z         Line end z coordinate
   \param[out]    *Rx         Nearest point on line x coordinate
   \param[out]    *Ry         Nearest point on line y coordinate
   \param[out]    *Rz         Nearest point on line z coordinate
   \param[out]    *frac       Fraction along P1-P2 of R
   \return                       Distance from P to R

   Calculates the shortest distance from a point P to a line between
   points P1 and P2. This value is returned.

   If the Rx,Ry,Rz pointers are all non-NULL, then the point on the
   line nearest to P is output.

   If the frac pointer is non-NULL then the fraction of R along the
   P1-P2 vector is output. Thus:
         R==P1 ==> frac=0
         R==P2 ==> frac=1
   Thus if (0<=frac<=1) then the point R is within the line segment
   P1-P2

-  16.11.99 Original   By: ACRM
-  07.07.14 Use bl prefix for functions By: CTP
*/
REAL blPointLineDistance(REAL Px, REAL Py, REAL Pz,
                         REAL P1x, REAL P1y, REAL P1z,
                         REAL P2x, REAL P2y, REAL P2z,
                         REAL *Rx, REAL *Ry, REAL *Rz,
                         REAL *frac)
{
   VEC3F A, u, Q, PQ, PR, QP, QP2;
   REAL  alen, len, f;
   
   
   /* Calculate vector from P1 to P2                                    */
   A.x = P2x - P1x;
   A.y = P2y - P1y;
   A.z = P2z - P1z;
   
   /* Calculate length of this vector                                   */
   alen = sqrt(DOTPRODUCT(A,A));

   /* If the two ends of the line are coincident then return the distance
      from either of them
   */
   if(alen==(REAL)0.0)
   {
      len = sqrt((Px-P1x)*(Px-P1x) + 
                 (Py-P1y)*(Py-P1y) + 
                 (Pz-P1z)*(Pz-P1z));
      if(frac!=NULL)
         *frac = 0.0;
      if(Rx != NULL && Ry != NULL && Rz != NULL)
      {
         *Rx = P1x;
         *Ry = P1y;
         *Rz = P1z;
      }

      return(len);
   }

   /* Calculate the unit vector along A                                 */
   u.x = A.x / alen;
   u.y = A.y / alen;
   u.z = A.z / alen;
   
   /* Select Q as any point on A, we'll make it P1                      */
   Q.x = P1x;
   Q.y = P1y;
   Q.z = P1z;
   
   /* Calculate vector PQ                                               */
   PQ.x = Q.x - Px;
   PQ.y = Q.y - Py;
   PQ.z = Q.z - Pz;

   /* Vector PR is the cross product of PQ and the unit vector
      along A (i.e. u)
   */
   CROSSPRODUCT(PQ, u, PR);
   
   /* And the length of that vector is the length we want               */
   len = sqrt(DOTPRODUCT(PR,PR));


   if(frac != NULL || Rx != NULL || Ry != NULL || Rz != NULL)
   {
      /*** OK we now know how far the point is from the line, so we   ***
       *** now want to calculate where the closest point (R) on the   ***
       *** line is to point P                                         ***/

      /* Find the projection of QP onto QP2                             */
      QP.x = Px - Q.x;
      QP.y = Py - Q.y;
      QP.z = Pz - Q.z;
      
      QP2.x = P2x - Q.x;
      QP2.y = P2y - Q.y;
      QP2.z = P2z - Q.z;
      
      f = DOTPRODUCT(QP, QP2) / sqrt(DOTPRODUCT(QP2, QP2));
      if(frac != NULL)
      {
         *frac = f/alen;
      }
      
      /* Find point R: this is the fraction f of the unit vector along 
         P1-P2 added onto Q 
      */
      if(Rx != NULL && Ry != NULL && Rz != NULL)
      {
         *Rx = Q.x + f * u.x;
         *Ry = Q.y + f * u.y;
         *Rz = Q.z + f * u.z;
      }
   }
   
   return(len);
}