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