Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/convention/src/lib/math/vector.cc @ 3756

Last change on this file since 3756 was 3607, checked in by patrick, 20 years ago

orxonox/trunk: stdincl was still included everywhere. removed it out of the stdincl.h file to enable the possibility of compile-speedup. added some testclasses for vector/quaternion.

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