Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/soundEngine/src/lib/math/vector.cc @ 3513

Last change on this file since 3513 was 3473, checked in by patrick, 20 years ago

orxonox/trunk: redesigning directory structure - created mathlib and added all important classes

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