Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/lib/math/vector.cc @ 4335

Last change on this file since 4335 was 3971, checked in by bensch, 20 years ago

orxonox/trunk: return value of quat-slerp was wrong

File size: 18.0 KB
Line 
1/*
2   orxonox - the future of 3D-vertical-scrollers
3
4   Copyright (C) 2004 orx
5
6   This program is free software; you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation; either version 2, or (at your option)
9   any later version.
10
11   ### File Specific:
12   main-programmer: Christian Meyer
13   co-programmer: Patrick Boenzli : Vector::scale()
14                                    Vector::abs()
15   
16   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
17*/
18
19#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
20
21#include "vector.h"
22#include "debug.h"
23
24using namespace std;
25
26/**
27   \brief add two vectors
28   \param v: the other vector
29   \return the sum of both vectors
30*/
31
32//Vector Vector::operator+ (const Vector& v) const
33
34
35/**
36   \brief subtract a vector from another
37   \param v: the other vector
38   \return the difference between the vectors
39*/
40//Vector Vector::operator- (const Vector& v) const
41
42
43/**
44   \brief calculate the dot product of two vectors
45   \param v: the other vector
46   \return the dot product of the vectors
47*/
48//float Vector::operator* (const Vector& v) const
49
50
51/**
52   \brief multiply a vector with a float
53   \param f: the factor
54   \return the vector multipied by f
55*/
56//Vector Vector::operator* (float f) const
57
58
59/**
60   \brief divide a vector with a float
61   \param f: the divisor
62   \return the vector divided by f   
63*/
64Vector Vector::operator/ (float f) const
65{
66  if( unlikely(f == 0.0))
67  {
68    // Prevent divide by zero
69    return Vector (0,0,0);
70  }
71  return Vector(x / f, y / f, z / f);
72}
73
74/**
75   \brief calculate the dot product of two vectors
76   \param v: the other vector
77   \return the dot product of the vectors
78*/
79float Vector::dot (const Vector& v) const
80{
81  return x*v.x+y*v.y+z*v.z;
82}
83
84/**
85  \brief calculate the cross product of two vectors
86  \param v: the other vector
87        \return the cross product of the vectors
88*/
89//Vector Vector::cross (const Vector& v) const
90
91 
92/**
93   \brief normalizes the vector to lenght 1.0
94*/
95//void Vector::normalize ()
96
97
98/**
99   \brief returns the voctor normalized to length 1.0
100*/
101
102Vector Vector::getNormalized()
103{
104  float l = len();
105  if(unlikely(l != 1.0))
106    {
107      return *this;
108    }
109  else if(unlikely(l == 0.0))
110    {
111      return *this;
112    }
113
114  return *this / l;
115}
116
117/**
118   \brief scales this Vector with Vector v.
119   \param v the vector to scale this vector with
120*/
121void Vector::scale(const Vector& v)
122{
123  x *= v.x;
124  y *= v.y;
125  z *= v.z;
126}
127
128 
129/**
130   \brief calculates the lenght of the vector
131   \return the lenght of the vector
132*/
133//float Vector::len () const
134
135
136/**
137   \brief Vector is looking in the positive direction on all axes after this
138*/
139Vector Vector::abs() 
140{
141  Vector v(fabs(x), fabs(y), fabs(z));
142  return v;
143}
144
145/**
146   \brief calculate the angle between two vectors in radiances
147   \param v1: a vector
148   \param v2: another vector
149   \return the angle between the vectors in radians
150*/
151float angleRad (const Vector& v1, const Vector& v2)
152{
153  return acos( v1 * v2 / (v1.len() * v2.len()));
154}
155
156
157/**
158   \brief calculate the angle between two vectors in degrees
159   \param v1: a vector
160   \param v2: another vector
161   \return the angle between the vectors in degrees
162*/
163float angleDeg (const Vector& v1, const Vector& v2)
164{
165  float f;
166  f = acos( v1 * v2 / (v1.len() * v2.len()));
167  return f * 180 / PI;
168}
169
170
171/**
172   \brief Outputs the values of the Vector
173*/
174void Vector::debug(void) const
175{
176  PRINT(0)("Vector Debug information\n");
177  PRINT(0)("x: %f; y: %f; z: %f", x, y, z);
178  PRINT(3)(" lenght: %f", len());
179  PRINT(0)("\n");
180}
181
182/**
183        \brief creates a multiplicational identity Quaternion
184*/
185//Quaternion::Quaternion ()
186
187
188/**
189        \brief turns a rotation along an axis into a Quaternion
190        \param angle: the amount of radians to rotate
191        \param axis: the axis to rotate around
192*/
193//Quaternion::Quaternion (float angle, const Vector& axis)
194
195
196/**
197   \brief calculates a lookAt rotation
198   \param dir: the direction you want to look
199   \param up: specify what direction up should be
200   
201   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
202   the same way as dir. If you want to use this with cameras, you'll have to reverse the
203   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
204   can use this for meshes as well (then you do not have to reverse the vector), but keep
205   in mind that if you do that, the model's front has to point in +z direction, and left
206   and right should be -x or +x respectively or the mesh wont rotate correctly.
207*/
208Quaternion::Quaternion (const Vector& dir, const Vector& up)
209{
210  Vector z = dir;
211  z.normalize(); 
212  Vector x = up.cross(z);
213  x.normalize(); 
214  Vector y = z.cross(x);
215 
216  float m[4][4];
217  m[0][0] = x.x;
218  m[0][1] = x.y;
219  m[0][2] = x.z;
220  m[0][3] = 0;
221  m[1][0] = y.x;
222  m[1][1] = y.y;
223  m[1][2] = y.z;
224  m[1][3] = 0;
225  m[2][0] = z.x;
226  m[2][1] = z.y;
227  m[2][2] = z.z;
228  m[2][3] = 0;
229  m[3][0] = 0;
230  m[3][1] = 0;
231  m[3][2] = 0;
232  m[3][3] = 1;
233 
234  *this = Quaternion (m);
235}
236
237/**
238        \brief calculates a rotation from euler angles
239        \param roll: the roll in radians
240        \param pitch: the pitch in radians
241        \param yaw: the yaw in radians
242       
243        I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know
244        in what order they are applied, I just copy-pasted the code.
245*/
246Quaternion::Quaternion (float roll, float pitch, float yaw)
247{
248        float cr, cp, cy, sr, sp, sy, cpcy, spsy;
249       
250        // calculate trig identities
251        cr = cos(roll/2);
252        cp = cos(pitch/2);
253        cy = cos(yaw/2);
254       
255        sr = sin(roll/2);
256        sp = sin(pitch/2);
257        sy = sin(yaw/2);
258       
259        cpcy = cp * cy;
260        spsy = sp * sy;
261       
262        w = cr * cpcy + sr * spsy;
263        v.x = sr * cpcy - cr * spsy;
264        v.y = cr * sp * cy + sr * cp * sy;
265        v.z = cr * cp * sy - sr * sp * cy;
266}
267
268/**
269        \brief rotates one Quaternion by another
270        \param q: another Quaternion to rotate this by
271        \return a quaternion that represents the first one rotated by the second one (WARUNING: this operation is not commutative! e.g. (A*B) != (B*A))
272*/
273Quaternion Quaternion::operator*(const Quaternion& q) const
274{
275  float A, B, C, D, E, F, G, H;
276 
277  A = (w   + v.x)*(q.w   + q.v.x);
278  B = (v.z - v.y)*(q.v.y - q.v.z);
279  C = (w   - v.x)*(q.v.y + q.v.z); 
280  D = (v.y + v.z)*(q.w   - q.v.x);
281  E = (v.x + v.z)*(q.v.x + q.v.y);
282  F = (v.x - v.z)*(q.v.x - q.v.y);
283  G = (w   + v.y)*(q.w   - q.v.z);
284  H = (w   - v.y)*(q.w   + q.v.z);
285 
286  Quaternion r;
287  r.v.x = A - (E + F + G + H)/2;
288  r.v.y = C + (E - F + G - H)/2;
289  r.v.z = D + (E - F - G + H)/2;
290  r.w = B +  (-E - F + G + H)/2;
291
292  return r;
293}
294
295/**
296   \brief add two Quaternions
297   \param q: another Quaternion
298   \return the sum of both Quaternions
299*/
300/*
301Quaternion Quaternion::operator+(const Quaternion& q) const
302{
303  Quaternion r(*this);
304  r.w = r.w + q.w;
305  r.v = r.v + q.v;
306  return r;
307}
308*/
309
310/**
311   \brief subtract two Quaternions
312   \param q: another Quaternion
313   \return the difference of both Quaternions
314*/
315/*
316Quaternion Quaternion::operator- (const Quaternion& q) const
317{
318  Quaternion r(*this);
319  r.w = r.w - q.w;
320  r.v = r.v - q.v;
321  return r;
322}
323*/
324
325/**
326   \brief rotate a Vector by a Quaternion
327   \param v: the Vector
328   \return a new Vector representing v rotated by the Quaternion
329*/
330
331Vector Quaternion::apply (Vector& v) const
332{
333  Quaternion q;
334  q.v = v;
335  q.w = 0;
336  q = *this * q * conjugate();
337  return q.v;
338}
339
340
341/**
342   \brief multiply a Quaternion with a real value
343   \param f: a real value
344   \return a new Quaternion containing the product
345*/
346Quaternion Quaternion::operator*(const float& f) const
347{
348  Quaternion r(*this);
349  r.w = r.w*f;
350  r.v = r.v*f;
351  return r;
352}
353
354/**
355   \brief divide a Quaternion by a real value
356   \param f: a real value
357   \return a new Quaternion containing the quotient
358*/
359Quaternion Quaternion::operator/(const float& f) const
360{
361  if( f == 0) return Quaternion();
362  Quaternion r(*this);
363  r.w = r.w/f;
364  r.v = r.v/f;
365  return r;
366}
367
368/**
369   \brief calculate the conjugate value of the Quaternion
370   \return the conjugate Quaternion
371*/
372/*
373Quaternion Quaternion::conjugate() const
374{
375  Quaternion r(*this);
376  r.v = Vector() - r.v;
377  return r;
378}
379*/
380
381/**
382   \brief calculate the norm of the Quaternion
383   \return the norm of The Quaternion
384*/
385float Quaternion::norm() const
386{
387  return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
388}
389
390/**
391   \brief calculate the inverse value of the Quaternion
392   \return the inverse Quaternion
393   
394        Note that this is equal to conjugate() if the Quaternion's norm is 1
395*/
396Quaternion Quaternion::inverse() const
397{
398  float n = norm();
399  if (n != 0)
400    {
401      return conjugate() / norm();
402    }
403  else return Quaternion();
404}
405
406/**
407   \brief convert the Quaternion to a 4x4 rotational glMatrix
408   \param m: a buffer to store the Matrix in
409*/
410void Quaternion::matrix (float m[4][4]) const
411{
412  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
413 
414  // calculate coefficients
415  x2 = v.x + v.x;
416  y2 = v.y + v.y; 
417  z2 = v.z + v.z;
418  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
419  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
420  wx = w * x2; wy = w * y2; wz = w * z2;
421 
422  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
423  m[2][0] = xz + wy; m[3][0] = 0.0;
424       
425  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
426  m[2][1] = yz - wx; m[3][1] = 0.0;
427 
428  m[0][2] = xz - wy; m[1][2] = yz + wx;
429  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
430 
431  m[0][3] = 0; m[1][3] = 0;
432  m[2][3] = 0; m[3][3] = 1;
433}
434
435/**
436   \brief performs a smooth move.
437   \param from  where
438   \param to where
439   \param t the time this transformation should take value [0..1]
440
441   \returns the Result of the smooth move
442*/
443Quaternion quatSlerp(const Quaternion& from, const Quaternion& to, float t)
444{
445  float tol[4];
446  double omega, cosom, sinom, scale0, scale1;
447  //  float DELTA = 0.2;
448
449  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
450
451  if( cosom < 0.0 ) 
452    { 
453      cosom = -cosom; 
454      tol[0] = -to.v.x;
455      tol[1] = -to.v.y;
456      tol[2] = -to.v.z;
457      tol[3] = -to.w;
458    }
459  else
460    {
461      tol[0] = to.v.x;
462      tol[1] = to.v.y;
463      tol[2] = to.v.z;
464      tol[3] = to.w;
465    }
466 
467  //if( (1.0 - cosom) > DELTA )
468  //{
469  omega = acos(cosom);
470  sinom = sin(omega);
471  scale0 = sin((1.0 - t) * omega) / sinom;
472  scale1 = sin(t * omega) / sinom;
473  //}
474  /*
475    else
476    {
477    scale0 = 1.0 - t;
478    scale1 = t;
479    }
480  */
481
482
483  /*
484    Quaternion res;
485    res.v.x = scale0 * from.v.x + scale1 * tol[0];
486    res.v.y = scale0 * from.v.y + scale1 * tol[1];
487    res.v.z = scale0 * from.v.z + scale1 * tol[2];
488    res.w = scale0 * from.w + scale1 * tol[3];
489  */
490  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
491                           scale0 * from.v.y + scale1 * tol[1],
492                           scale0 * from.v.z + scale1 * tol[2]),
493                    scale0 * from.w + scale1 * tol[3]);
494}
495
496
497/**
498   \brief convert a rotational 4x4 glMatrix into a Quaternion
499   \param m: a 4x4 matrix in glMatrix order
500*/
501Quaternion::Quaternion (float m[4][4])
502{
503       
504  float  tr, s, q[4];
505  int    i, j, k;
506
507  int nxt[3] = {1, 2, 0};
508
509  tr = m[0][0] + m[1][1] + m[2][2];
510
511        // check the diagonal
512  if (tr > 0.0) 
513  {
514    s = sqrt (tr + 1.0);
515    w = s / 2.0;
516    s = 0.5 / s;
517    v.x = (m[1][2] - m[2][1]) * s;
518    v.y = (m[2][0] - m[0][2]) * s;
519    v.z = (m[0][1] - m[1][0]) * s;
520        } 
521        else 
522        {
523                // diagonal is negative
524        i = 0;
525        if (m[1][1] > m[0][0]) i = 1;
526    if (m[2][2] > m[i][i]) i = 2;
527    j = nxt[i];
528    k = nxt[j];
529
530    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
531     
532    q[i] = s * 0.5;
533           
534    if (s != 0.0) s = 0.5 / s;
535       
536          q[3] = (m[j][k] - m[k][j]) * s;
537    q[j] = (m[i][j] + m[j][i]) * s;
538    q[k] = (m[i][k] + m[k][i]) * s;
539
540        v.x = q[0];
541        v.y = q[1];
542        v.z = q[2];
543        w = q[3];
544  }
545}
546
547/**
548   \brief outputs some nice formated debug information about this quaternion
549*/
550void Quaternion::debug(void)
551{
552  PRINT(0)("Quaternion Debug Information\n");
553  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
554}
555
556/**
557   \brief create a rotation from a vector
558   \param v: a vector
559*/
560Rotation::Rotation (const Vector& v)
561{
562  Vector x = Vector( 1, 0, 0);
563  Vector axis = x.cross( v);
564  axis.normalize();
565  float angle = angleRad( x, v);
566  float ca = cos(angle);
567  float sa = sin(angle);
568  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
569  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
570  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
571  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
572  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
573  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
574  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
575  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
576  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
577}
578
579/**
580   \brief creates a rotation from an axis and an angle (radians!)
581   \param axis: the rotational axis
582   \param angle: the angle in radians
583*/
584Rotation::Rotation (const Vector& axis, float angle)
585{
586  float ca, sa;
587  ca = cos(angle);
588  sa = sin(angle);
589  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
590  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
591  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
592  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
593  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
594  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
595  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
596  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
597  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
598}
599
600/**
601   \brief creates a rotation from euler angles (pitch/yaw/roll)
602   \param pitch: rotation around z (in radians)
603   \param yaw: rotation around y (in radians)
604   \param roll: rotation around x (in radians)
605*/
606Rotation::Rotation ( float pitch, float yaw, float roll)
607{
608  float cy, sy, cr, sr, cp, sp;
609  cy = cos(yaw);
610  sy = sin(yaw);
611  cr = cos(roll);
612  sr = sin(roll);
613  cp = cos(pitch);
614  sp = sin(pitch);
615  m[0] = cy*cr;
616  m[1] = -cy*sr;
617  m[2] = sy;
618  m[3] = cp*sr+sp*sy*cr;
619  m[4] = cp*cr-sp*sr*sy;
620  m[5] = -sp*cy;
621  m[6] = sp*sr-cp*sy*cr;
622  m[7] = sp*cr+cp*sy*sr;
623  m[8] = cp*cy;
624}
625
626/**
627   \brief creates a nullrotation (an identity rotation)
628*/
629Rotation::Rotation ()
630{
631  m[0] = 1.0f;
632  m[1] = 0.0f;
633  m[2] = 0.0f;
634  m[3] = 0.0f;
635  m[4] = 1.0f;
636  m[5] = 0.0f;
637  m[6] = 0.0f;
638  m[7] = 0.0f;
639  m[8] = 1.0f;
640}
641
642/**
643   \brief fills the specified buffer with a 4x4 glmatrix
644   \param buffer: Pointer to an array of 16 floats
645   
646   Use this to get the rotation in a gl-compatible format
647*/
648void Rotation::glmatrix (float* buffer)
649{
650        buffer[0] = m[0];
651        buffer[1] = m[3];
652        buffer[2] = m[6];
653        buffer[3] = m[0];
654        buffer[4] = m[1];
655        buffer[5] = m[4];
656        buffer[6] = m[7];
657        buffer[7] = m[0];
658        buffer[8] = m[2];
659        buffer[9] = m[5];
660        buffer[10] = m[8];
661        buffer[11] = m[0];
662        buffer[12] = m[0];
663        buffer[13] = m[0];
664        buffer[14] = m[0];
665        buffer[15] = m[1];
666}
667
668/**
669   \brief multiplies two rotational matrices
670   \param r: another Rotation
671   \return the matrix product of the Rotations
672   
673   Use this to rotate one rotation by another
674*/
675Rotation Rotation::operator* (const Rotation& r)
676{
677        Rotation p;
678       
679        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
680        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
681        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
682       
683        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
684        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
685        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
686
687        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
688        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
689        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
690       
691        return p;
692}
693
694
695/**
696   \brief rotates the vector by the given rotation
697   \param v: a vector
698   \param r: a rotation
699   \return the rotated vector
700*/
701Vector rotateVector( const Vector& v, const Rotation& r)
702{
703  Vector t;
704 
705  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
706  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
707  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
708
709  return t;
710}
711
712/**
713   \brief calculate the distance between two lines
714   \param l: the other line
715   \return the distance between the lines
716*/
717float Line::distance (const Line& l) const
718{
719  float q, d;
720  Vector n = a.cross(l.a);
721  q = n.dot(r-l.r);
722  d = n.len();
723  if( d == 0.0) return 0.0;
724  return q/d;
725}
726
727/**
728   \brief calculate the distance between a line and a point
729   \param v: the point
730   \return the distance between the Line and the point
731*/
732float Line::distancePoint (const Vector& v) const
733{
734  Vector d = v-r;
735  Vector u = a * d.dot( a);
736  return (d - u).len();
737}
738
739/**
740   \brief calculate the two points of minimal distance of two lines
741   \param l: the other line
742   \return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
743*/
744Vector* Line::footpoints (const Line& l) const
745{
746  Vector* fp = new Vector[2];
747  Plane p = Plane (r + a.cross(l.a), r, r + a);
748  fp[1] = p.intersectLine (l);
749  p = Plane (fp[1], l.a);
750  fp[0] = p.intersectLine (*this);
751  return fp;
752}
753
754/**
755  \brief calculate the length of a line
756  \return the lenght of the line
757*/
758float Line::len() const
759{
760  return a.len();
761}
762
763/**
764   \brief rotate the line by given rotation
765   \param rot: a rotation
766*/
767void Line::rotate (const Rotation& rot)
768{
769  Vector t = a + r;
770  t = rotateVector( t, rot);
771  r = rotateVector( r, rot),
772  a = t - r;
773}
774
775/**
776   \brief create a plane from three points
777   \param a: first point
778   \param b: second point
779   \param c: third point
780*/
781Plane::Plane (Vector a, Vector b, Vector c)
782{
783  n = (a-b).cross(c-b);
784  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
785}
786
787/**
788   \brief create a plane from anchor point and normal
789   \param norm: normal vector
790   \param p: anchor point
791*/
792Plane::Plane (Vector norm, Vector p)
793{
794  n = norm;
795  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
796}
797
798/**
799   \brief returns the intersection point between the plane and a line
800   \param l: a line
801*/
802Vector Plane::intersectLine (const Line& l) const
803{
804  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
805  float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z);
806  return l.r + (l.a * t);
807}
808
809/**
810   \brief returns the distance between the plane and a point
811   \param p: a Point
812   \return the distance between the plane and the point (can be negative)
813*/
814float Plane::distancePoint (const Vector& p) const
815{
816  float l = n.len();
817  if( l == 0.0) return 0.0;
818  return (n.dot(p) + k) / n.len();
819}
820
821/**
822   \brief returns the side a point is located relative to a Plane
823   \param p: a Point
824   \return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane
825*/
826float Plane::locatePoint (const Vector& p) const
827{
828  return (n.dot(p) + k);
829}
830
Note: See TracBrowser for help on using the repository browser.