Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/vector.cc @ 3009

Last change on this file since 3009 was 3004, checked in by bensch, 20 years ago

orxonox/trunk: implemented Bezier-Diff

File size: 20.0 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   \bref 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
173void Vector::scale(const Vector& v)
174{
175  x *= v.x;
176  y *= v.y;
177  z *= v.z;
178}
179
180 
181/**
182   \brief calculates the lenght of the vector
183   \return the lenght of the vector
184*/
185float Vector::len () const
186{
187  return sqrt (x*x+y*y+z*z);
188}
189
190
191
192Vector Vector::abs() 
193{
194  Vector v(fabs(x), fabs(y), fabs(z));
195  return v;
196}
197
198
199/**
200   \brief calculate the angle between two vectors in radiances
201   \param v1: a vector
202   \param v2: another vector
203   \return the angle between the vectors in radians
204*/
205float angle_rad (const Vector& v1, const Vector& v2)
206{
207  return acos( v1 * v2 / (v1.len() * v2.len()));
208}
209
210
211/**
212   \brief calculate the angle between two vectors in degrees
213   \param v1: a vector
214   \param v2: another vector
215   \return the angle between the vectors in degrees
216*/
217float angle_deg (const Vector& v1, const Vector& v2)
218{
219  float f;
220  f = acos( v1 * v2 / (v1.len() * v2.len()));
221  return f * 180 / PI;
222}
223
224/**
225        \brief creates a multiplicational identity Quaternion
226*/
227Quaternion::Quaternion ()
228{
229        w = 1;
230        v = Vector(0,0,0);
231}
232
233/**
234        \brief turns a rotation along an axis into a Quaternion
235        \param angle: the amount of radians to rotate
236        \param axis: the axis to rotate around
237*/
238Quaternion::Quaternion (float angle, const Vector& axis)
239{
240        w = cos(angle/2);
241        v = axis * sin(angle/2);
242}
243
244/**
245   \brief calculates a look_at rotation
246   \param dir: the direction you want to look
247   \param up: specify what direction up should be
248   
249   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
250   the same way as dir. If you want to use this with cameras, you'll have to reverse the
251   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
252   can use this for meshes as well (then you do not have to reverse the vector), but keep
253   in mind that if you do that, the model's front has to point in +z direction, and left
254   and right should be -x or +x respectively or the mesh wont rotate correctly.
255*/
256Quaternion::Quaternion (const Vector& dir, const Vector& up)
257{
258  Vector z = dir;
259  z.normalize(); 
260  Vector x = up.cross(z);
261  x.normalize(); 
262  Vector y = z.cross(x);
263 
264  float m[4][4];
265  m[0][0] = x.x;
266  m[0][1] = x.y;
267  m[0][2] = x.z;
268  m[0][3] = 0;
269  m[1][0] = y.x;
270  m[1][1] = y.y;
271  m[1][2] = y.z;
272  m[1][3] = 0;
273  m[2][0] = z.x;
274  m[2][1] = z.y;
275  m[2][2] = z.z;
276  m[2][3] = 0;
277  m[3][0] = 0;
278  m[3][1] = 0;
279  m[3][2] = 0;
280  m[3][3] = 1;
281 
282  *this = Quaternion (m);
283}
284
285/**
286        \brief calculates a rotation from euler angles
287        \param roll: the roll in radians
288        \param pitch: the pitch in radians
289        \param yaw: the yaw in radians
290       
291        I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know
292        in what order they are applied, I just copy-pasted the code.
293*/
294Quaternion::Quaternion (float roll, float pitch, float yaw)
295{
296        float cr, cp, cy, sr, sp, sy, cpcy, spsy;
297       
298        // calculate trig identities
299        cr = cos(roll/2);
300        cp = cos(pitch/2);
301        cy = cos(yaw/2);
302       
303        sr = sin(roll/2);
304        sp = sin(pitch/2);
305        sy = sin(yaw/2);
306       
307        cpcy = cp * cy;
308        spsy = sp * sy;
309       
310        w = cr * cpcy + sr * spsy;
311        v.x = sr * cpcy - cr * spsy;
312        v.y = cr * sp * cy + sr * cp * sy;
313        v.z = cr * cp * sy - sr * sp * cy;
314}
315
316/**
317        \brief rotates one Quaternion by another
318        \param q: another Quaternion to rotate this by
319        \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))
320*/
321Quaternion Quaternion::operator*(const Quaternion& q) const
322{
323        float A, B, C, D, E, F, G, H;
324        Quaternion r;
325       
326        A = (w   + v.x)*(q.w   + q.v.x);
327        B = (v.z - v.y)*(q.v.y - q.v.z);
328        C = (w   - v.x)*(q.v.y + q.v.z); 
329        D = (v.y + v.z)*(q.w   - q.v.x);
330        E = (v.x + v.z)*(q.v.x + q.v.y);
331        F = (v.x - v.z)*(q.v.x - q.v.y);
332        G = (w   + v.y)*(q.w   - q.v.z);
333        H = (w   - v.y)*(q.w   + q.v.z);
334       
335        r.w = B +  (-E - F + G + H)/2;
336        r.v.x = A - (E + F + G + H)/2; 
337        r.v.y = C + (E - F + G - H)/2; 
338        r.v.z = D + (E - F - G + H)/2;
339       
340        return r;
341}
342
343/**
344        \brief add two Quaternions
345        \param q: another Quaternion
346        \return the sum of both Quaternions
347*/
348Quaternion Quaternion::operator+(const Quaternion& q) const
349{
350        Quaternion r(*this);
351        r.w = r.w + q.w;
352        r.v = r.v + q.v;
353        return r;
354}
355
356/**
357        \brief subtract two Quaternions
358        \param q: another Quaternion
359        \return the difference of both Quaternions
360*/
361Quaternion Quaternion::operator- (const Quaternion& q) const
362{
363        Quaternion r(*this);
364        r.w = r.w - q.w;
365        r.v = r.v - q.v;
366        return r;
367}
368
369/**
370        \brief rotate a Vector by a Quaternion
371        \param v: the Vector
372        \return a new Vector representing v rotated by the Quaternion
373*/
374Vector Quaternion::apply (Vector& v) const
375{
376        Quaternion q;
377        q.v = v;
378        q.w = 0;
379        q = *this * q * conjugate();
380        return q.v;
381}
382
383/**
384        \brief multiply a Quaternion with a real value
385        \param f: a real value
386        \return a new Quaternion containing the product
387*/
388Quaternion Quaternion::operator*(const float& f) const
389{
390        Quaternion r(*this);
391        r.w = r.w*f;
392        r.v = r.v*f;
393        return r;
394}
395
396/**
397        \brief divide a Quaternion by a real value
398        \param f: a real value
399        \return a new Quaternion containing the quotient
400*/
401Quaternion Quaternion::operator/(const float& f) const
402{
403        if( f == 0) return Quaternion();
404        Quaternion r(*this);
405        r.w = r.w/f;
406        r.v = r.v/f;
407        return r;
408}
409
410/**
411        \brief calculate the conjugate value of the Quaternion
412        \return the conjugate Quaternion
413*/
414Quaternion Quaternion::conjugate() const
415{
416        Quaternion r(*this);
417        r.v = Vector() - r.v;
418        return r;
419}
420
421/**
422        \brief calculate the norm of the Quaternion
423        \return the norm of The Quaternion
424*/
425float Quaternion::norm() const
426{
427        return w*w + v.x*v.x + v.y*v.y + v.z*v.z;
428}
429
430/**
431        \brief calculate the inverse value of the Quaternion
432        \return the inverse Quaternion
433       
434        Note that this is equal to conjugate() if the Quaternion's norm is 1
435*/
436Quaternion Quaternion::inverse() const
437{
438        float n = norm();
439        if (n != 0)
440        {
441                return conjugate() / norm();
442        }
443        else return Quaternion();
444}
445
446/**
447        \brief convert the Quaternion to a 4x4 rotational glMatrix
448        \param m: a buffer to store the Matrix in
449*/
450void Quaternion::matrix (float m[4][4]) const
451{
452  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 
453 
454  // calculate coefficients
455  x2 = v.x + v.x;
456  y2 = v.y + v.y; 
457  z2 = v.z + v.z;
458  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
459  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
460  wx = w * x2; wy = w * y2; wz = w * z2;
461 
462  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
463  m[2][0] = xz + wy; m[3][0] = 0.0;
464       
465  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
466  m[2][1] = yz - wx; m[3][1] = 0.0;
467 
468  m[0][2] = xz - wy; m[1][2] = yz + wx;
469  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
470 
471  m[0][3] = 0; m[1][3] = 0;
472  m[2][3] = 0; m[3][3] = 1;
473}
474
475
476void Quaternion::quatSlerp(const Quaternion* from, const Quaternion* to, float t, Quaternion* res)
477{
478  float tol[4];
479  double omega, cosom, sinom, scale0, scale1;
480  DELTA = 0.2;
481
482  cosom = from->v.x * to->v.x + from->v.y * to->v.y + from->v.z * to->v.z + from->w * to->w;
483
484  if( cosom < 0.0 ) 
485    { 
486      cosom = -cosom; 
487      tol[0] = -to->v.x;
488      tol[1] = -to->v.y;
489      tol[2] = -to->v.z;
490      tol[3] = -to->w;
491    }
492  else
493    {
494      tol[0] = to->v.x;
495      tol[1] = to->v.y;
496      tol[2] = to->v.z;
497      tol[3] = to->w;
498    }
499 
500  //if( (1.0 - cosom) > DELTA )
501  //{
502      omega = acos(cosom);
503      sinom = sin(omega);
504      scale0 = sin((1.0 - t) * omega) / sinom;
505      scale1 = sin(t * omega) / sinom;
506      //}
507      /*
508  else
509    {
510      scale0 = 1.0 - t;
511      scale1 = t;
512    }
513      */
514  res->v.x = scale0 * from->v.x + scale1 * tol[0];
515  res->v.y = scale0 * from->v.y + scale1 * tol[1];
516  res->v.z = scale0 * from->v.z + scale1 * tol[2];
517  res->w = scale0 * from->w + scale1 * tol[3];
518}
519
520
521/**
522   \brief convert a rotational 4x4 glMatrix into a Quaternion
523   \param m: a 4x4 matrix in glMatrix order
524*/
525Quaternion::Quaternion (float m[4][4])
526{
527       
528  float  tr, s, q[4];
529  int    i, j, k;
530
531  int nxt[3] = {1, 2, 0};
532
533  tr = m[0][0] + m[1][1] + m[2][2];
534
535        // check the diagonal
536  if (tr > 0.0) 
537  {
538    s = sqrt (tr + 1.0);
539    w = s / 2.0;
540    s = 0.5 / s;
541    v.x = (m[1][2] - m[2][1]) * s;
542    v.y = (m[2][0] - m[0][2]) * s;
543    v.z = (m[0][1] - m[1][0]) * s;
544        } 
545        else 
546        {
547                // diagonal is negative
548        i = 0;
549        if (m[1][1] > m[0][0]) i = 1;
550    if (m[2][2] > m[i][i]) i = 2;
551    j = nxt[i];
552    k = nxt[j];
553
554    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
555     
556    q[i] = s * 0.5;
557           
558    if (s != 0.0) s = 0.5 / s;
559       
560          q[3] = (m[j][k] - m[k][j]) * s;
561    q[j] = (m[i][j] + m[j][i]) * s;
562    q[k] = (m[i][k] + m[k][i]) * s;
563
564        v.x = q[0];
565        v.y = q[1];
566        v.z = q[2];
567        w = q[3];
568  }
569}
570
571/**
572   \brief create a rotation from a vector
573   \param v: a vector
574*/
575Rotation::Rotation (const Vector& v)
576{
577  Vector x = Vector( 1, 0, 0);
578  Vector axis = x.cross( v);
579  axis.normalize();
580  float angle = angle_rad( x, v);
581  float ca = cos(angle);
582  float sa = sin(angle);
583  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
584  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
585  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
586  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
587  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
588  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
589  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
590  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
591  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
592}
593
594/**
595   \brief creates a rotation from an axis and an angle (radians!)
596   \param axis: the rotational axis
597   \param angle: the angle in radians
598*/
599Rotation::Rotation (const Vector& axis, float angle)
600{
601  float ca, sa;
602  ca = cos(angle);
603  sa = sin(angle);
604  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
605  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
606  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
607  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
608  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
609  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
610  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
611  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
612  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
613}
614
615/**
616   \brief creates a rotation from euler angles (pitch/yaw/roll)
617   \param pitch: rotation around z (in radians)
618   \param yaw: rotation around y (in radians)
619   \param roll: rotation around x (in radians)
620*/
621Rotation::Rotation ( float pitch, float yaw, float roll)
622{
623  float cy, sy, cr, sr, cp, sp;
624  cy = cos(yaw);
625  sy = sin(yaw);
626  cr = cos(roll);
627  sr = sin(roll);
628  cp = cos(pitch);
629  sp = sin(pitch);
630  m[0] = cy*cr;
631  m[1] = -cy*sr;
632  m[2] = sy;
633  m[3] = cp*sr+sp*sy*cr;
634  m[4] = cp*cr-sp*sr*sy;
635  m[5] = -sp*cy;
636  m[6] = sp*sr-cp*sy*cr;
637  m[7] = sp*cr+cp*sy*sr;
638  m[8] = cp*cy;
639}
640
641/**
642   \brief creates a nullrotation (an identity rotation)
643*/
644Rotation::Rotation ()
645{
646  m[0] = 1.0f;
647  m[1] = 0.0f;
648  m[2] = 0.0f;
649  m[3] = 0.0f;
650  m[4] = 1.0f;
651  m[5] = 0.0f;
652  m[6] = 0.0f;
653  m[7] = 0.0f;
654  m[8] = 1.0f;
655}
656
657/**
658   \brief fills the specified buffer with a 4x4 glmatrix
659   \param buffer: Pointer to an array of 16 floats
660   
661   Use this to get the rotation in a gl-compatible format
662*/
663void Rotation::glmatrix (float* buffer)
664{
665        buffer[0] = m[0];
666        buffer[1] = m[3];
667        buffer[2] = m[6];
668        buffer[3] = m[0];
669        buffer[4] = m[1];
670        buffer[5] = m[4];
671        buffer[6] = m[7];
672        buffer[7] = m[0];
673        buffer[8] = m[2];
674        buffer[9] = m[5];
675        buffer[10] = m[8];
676        buffer[11] = m[0];
677        buffer[12] = m[0];
678        buffer[13] = m[0];
679        buffer[14] = m[0];
680        buffer[15] = m[1];
681}
682
683/**
684   \brief multiplies two rotational matrices
685   \param r: another Rotation
686   \return the matrix product of the Rotations
687   
688   Use this to rotate one rotation by another
689*/
690Rotation Rotation::operator* (const Rotation& r)
691{
692        Rotation p;
693       
694        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
695        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
696        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
697       
698        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
699        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
700        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
701
702        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
703        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
704        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
705       
706        return p;
707}
708
709
710/**
711   \brief rotates the vector by the given rotation
712   \param v: a vector
713   \param r: a rotation
714   \return the rotated vector
715*/
716Vector rotate_vector( const Vector& v, const Rotation& r)
717{
718  Vector t;
719 
720  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
721  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
722  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
723
724  return t;
725}
726
727/**
728   \brief calculate the distance between two lines
729   \param l: the other line
730   \return the distance between the lines
731*/
732float Line::distance (const Line& l) const
733{
734  float q, d;
735  Vector n = a.cross(l.a);
736  q = n.dot(r-l.r);
737  d = n.len();
738  if( d == 0.0) return 0.0;
739  return q/d;
740}
741
742/**
743   \brief calculate the distance between a line and a point
744   \param v: the point
745   \return the distance between the Line and the point
746*/
747float Line::distance_point (const Vector& v) const
748{
749  Vector d = v-r;
750  Vector u = a * d.dot( a);
751  return (d - u).len();
752}
753
754/**
755   \brief calculate the two points of minimal distance of two lines
756   \param l: the other line
757   \return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
758*/
759Vector* Line::footpoints (const Line& l) const
760{
761  Vector* fp = new Vector[2];
762  Plane p = Plane (r + a.cross(l.a), r, r + a);
763  fp[1] = p.intersect_line (l);
764  p = Plane (fp[1], l.a);
765  fp[0] = p.intersect_line (*this);
766  return fp;
767}
768
769/**
770  \brief calculate the length of a line
771  \return the lenght of the line
772*/
773float Line::len() const
774{
775  return a.len();
776}
777
778/**
779   \brief rotate the line by given rotation
780   \param rot: a rotation
781*/
782void Line::rotate (const Rotation& rot)
783{
784  Vector t = a + r;
785  t = rotate_vector( t, rot);
786  r = rotate_vector( r, rot),
787  a = t - r;
788}
789
790/**
791   \brief create a plane from three points
792   \param a: first point
793   \param b: second point
794   \param c: third point
795*/
796Plane::Plane (Vector a, Vector b, Vector c)
797{
798  n = (a-b).cross(c-b);
799  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
800}
801
802/**
803   \brief create a plane from anchor point and normal
804   \param n: normal vector
805   \param p: anchor point
806*/
807Plane::Plane (Vector norm, Vector p)
808{
809  n = norm;
810  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
811}
812
813/**
814   \brief returns the intersection point between the plane and a line
815   \param l: a line
816*/
817Vector Plane::intersect_line (const Line& l) const
818{
819  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
820  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);
821  return l.r + (l.a * t);
822}
823
824/**
825   \brief returns the distance between the plane and a point
826   \param p: a Point
827   \return the distance between the plane and the point (can be negative)
828*/
829float Plane::distance_point (const Vector& p) const
830{
831  float l = n.len();
832  if( l == 0.0) return 0.0;
833  return (n.dot(p) + k) / n.len();
834}
835
836/**
837   \brief returns the side a point is located relative to a Plane
838   \param p: a Point
839   \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
840*/
841float Plane::locate_point (const Vector& p) const
842{
843  return (n.dot(p) + k);
844}
845
846
847/**
848   \brief Creates a new BezierCurve
849*/
850BezierCurve::BezierCurve (void)
851{
852  nodeCount = 0;
853  firstNode = new PathNode;
854  currentNode = firstNode;
855
856  firstNode->position = Vector (.0, .0, .0);
857  firstNode->number = 0;
858  firstNode->next = 0; // not sure if this really points to NULL!!
859
860  return;
861}
862
863/**
864   \brief Deletes a BezierCurve.
865   It does this by freeing all the space taken over from the nodes
866*/
867BezierCurve::~BezierCurve (void)
868{
869  PathNode* tmpNode;
870  currentNode = firstNode;
871  while (tmpNode != 0)
872    {
873      tmpNode = currentNode;
874      currentNode = currentNode->next;
875      delete tmpNode;
876    }
877}
878
879/**
880   \brief adds a new Node to the bezier Curve
881   \param newNode a Vector to the position of the new node
882*/
883void BezierCurve::addNode(const Vector& newNode)
884{
885  PathNode* tmpNode;
886  if (nodeCount == 0 )
887    tmpNode = firstNode;
888  else
889    {
890      tmpNode = new PathNode;
891      currentNode = currentNode->next = tmpNode;
892    }
893  tmpNode->position = newNode;
894  tmpNode->next = 0; // not sure if this really points to NULL!!
895  tmpNode->number = (++nodeCount);
896  return;
897}
898
899/**
900   \brief calculates the Position on the curve
901   \param t The position on the Curve (0<=t<=1)
902   \return the Position on the Path
903*/
904Vector BezierCurve::calcPos(float t) 
905{
906  if (nodeCount <=4)
907    {
908      //      if (verbose >= 1)
909      //      printf ("Please define at least 4 nodes, until now you have only defined %i.\n", nodeCount);
910      curvePoint = Vector(0,0,0);
911    }
912  PathNode* tmpNode = firstNode;
913   
914  int k,kn,nn,nkn;
915  double blend,muk,munk;
916  Vector b = Vector(0.0,0.0,0.0);
917 
918  muk = 1;
919  munk = pow(1-t,(double)nodeCount);
920 
921  for (k=0; k<=nodeCount; k++) {
922    nn = nodeCount;
923    kn = k;
924    nkn = nodeCount - k;
925    blend = muk * munk;
926    muk *= t;
927    munk /= (1-t);
928    while (nn >= 1) {
929      blend *= nn;
930      nn--;
931      if (kn > 1) {
932        blend /= (double)kn;
933        kn--;
934      }
935      if (nkn > 1) {
936        blend /= (double)nkn;
937        nkn--;
938      }
939    }
940    b.x += tmpNode->position.x * blend;
941    b.y += tmpNode->position.y * blend;
942    b.z += tmpNode->position.z * blend;
943
944    tmpNode = tmpNode->next;
945  }
946  return b;
947}
948
949Vector BezierCurve::calcDirection (float t)
950{
951  double diff = .00000000001;
952 
953  Vector diffV = ((calcPos(t+diff) - calcPos(t))/diff);
954  diffV.normalize();
955  return diffV;
956}
957
958/**
959  \brief returns the Position of the point calculated on the Curve
960  \return a Vector to the calculated position
961*/
962Vector BezierCurve::getPos() const
963{
964  return curvePoint;
965}
Note: See TracBrowser for help on using the repository browser.