Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/branches/nico/src/curve.cc @ 3943

Last change on this file since 3943 was 3399, checked in by bensch, 20 years ago

orxonox/branches/nico: merged trunk into branches/nico
merged with command:
svn merge ../trunk/ nico/ -r 3238:HEAD
resolved conflict in favor of branches/nico.

File size: 8.7 KB
RevLine 
[3018]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: Benjamin Grauer
[3399]13   co-programmer: Patrick Boenzli
[3023]14
[3399]15   ADD: Patrick Boenzli           B-Spline
16
17
[3023]18   TODO:
19     local-Time implementation
20     NURBS
21     
[3018]22*/
23
24#include "curve.h"
[3399]25#include "matrix.h"
26#include "debug.h"
[3018]27
[3399]28#include <math.h>
29#include <stdio.h>
[3019]30
[3018]31/**
[3019]32   \brief adds a new Node to the bezier Curve
33   \param newNode a Vector to the position of the new node
34*/
35void Curve::addNode(const Vector& newNode)
36{
37  if (nodeCount != 0 )
38    {
39      currentNode = currentNode->next = new PathNode;
40    }
41  currentNode->position = newNode;
42  currentNode->next = 0; // not sure if this really points to NULL!!
43  currentNode->number = (++nodeCount);
[3399]44  this->rebuild();
[3019]45  return;
46}
47
[3399]48/**
49   \brief Finds a Node by its Number, and returns its Position
50   \param nodeToFind the n'th node in the List of nodes
51   \returns A Vector to the Position of the Node.
52*/
53Vector Curve::getNode(unsigned int nodeToFind)
54{
55  if (nodeToFind > this->nodeCount)
56    return Vector(0,0,0);
57  PathNode* tmpNode = this->firstNode;
58  for (int i = 1; i < nodeToFind; i++)
59    tmpNode = tmpNode->next;
60  return tmpNode->position;
61}
[3019]62
[3399]63///////////////////////////////////
64/// Bezier Curve //////////////////
65///////////////////////////////////
66
[3019]67/**
[3018]68   \brief Creates a new BezierCurve
69*/
70BezierCurve::BezierCurve (void)
71{
[3399]72  this->derivation = 0;
73  dirCurve = new BezierCurve(1);
74  this->init();
75}
76
77/**
78   \brief Creates a new BezierCurve-Derivation-Curve
79*/
80BezierCurve::BezierCurve (int derivation)
81{
82  this->derivation = derivation;
83  dirCurve=NULL;
84  this->init();
85}
86
87/**
88   \brief Deletes a BezierCurve.
89
90   It does this by freeing all the space taken over from the nodes
91*/
92BezierCurve::~BezierCurve(void)
93{
94  PathNode* tmpNode;
95  currentNode = firstNode;
96  while (tmpNode != 0)
97    {
98      tmpNode = currentNode;
99      currentNode = currentNode->next;
100      delete tmpNode;
101    }
102  if (dirCurve)
103    delete dirCurve;
104}
105
106/**
107   \brief Initializes a BezierCurve
108*/
109void BezierCurve::init(void)
110{
[3018]111  nodeCount = 0;
112  firstNode = new PathNode;
113  currentNode = firstNode;
114
115  firstNode->position = Vector (.0, .0, .0);
116  firstNode->number = 0;
117  firstNode->next = 0; // not sure if this really points to NULL!!
118
119  return;
120}
121
122/**
[3399]123   \brief Rebuilds a Curve
124*/
125void BezierCurve::rebuild(void)
126{
127  PathNode* tmpNode = firstNode;
[3217]128
[3399]129  // rebuilding the Curve itself
130  float k=0;
131  float n = nodeCount -1;
132  float binCoef = 1;
133  while(tmpNode)
134    {
135      tmpNode->factor = binCoef;
136      if (tmpNode =tmpNode->next)
137        {
138          binCoef *=(n-k)/(k+1);
139          ++k;
140        }
141    }
142
143  // rebuilding the Derivation curve
144  if(this->derivation == 0)
145    {
146      tmpNode = firstNode;
147      delete dirCurve;
148      dirCurve = new BezierCurve(1);
149      while(tmpNode->next)
150        {
151          Vector tmpVector = (tmpNode->next->position)- (tmpNode->position);
152          tmpVector.x*=(float)nodeCount;
153          tmpVector.y*=(float)nodeCount;
154          tmpVector.z*=(float)nodeCount;
155          tmpVector.normalize();
156          this->dirCurve->addNode(tmpVector);
157          tmpNode = tmpNode->next;
158        }
159    }
160}
161
162/**
163   \brief calculates the Position on the curve
164   \param t The position on the Curve (0<=t<=1)
165   \return the Position on the Path
166*/
167Vector BezierCurve::calcPos(float t) 
168{
169  Vector ret = Vector(0.0,0.0,0.0);
170  if (this->nodeCount >= 3)
171    {
172      PathNode* tmpNode = this->firstNode;
173      double factor = pow(1.0-t,nodeCount-1);
174      while(tmpNode)
175        {
176          ret.x += tmpNode->factor * factor * tmpNode->position.x;
177          ret.y += tmpNode->factor * factor * tmpNode->position.y;
178          ret.z += tmpNode->factor * factor * tmpNode->position.z;
179          factor *= t/(1.0-t); // same as pow but much faster.
180         
181          tmpNode = tmpNode->next;
182        }
183    }
184  else if (nodeCount == 2)
185    {
186      ret = this->firstNode->position *(1.0-t);
187      ret = ret + this->firstNode->next->position * t;
188    }
189  else if (nodeCount == 1)
190    ret = this->firstNode->position;
191  return ret;
192}
193
194/**
195   \brief Calulates the direction of the Curve at time t.
196   \param The time at which to evaluate the curve.
197   \returns The vvaluated Vector.
198*/
199Vector BezierCurve::calcDir (float t)
200{
201  return dirCurve->calcPos(t);
202}
203
204/**
205   \brief Calculates the Quaternion needed for our rotations
206   \param t The time at which to evaluate the cuve.
207   \returns The evaluated Quaternion.
208*/
209Quaternion BezierCurve::calcQuat (float t)
210{
211  return Quaternion (calcDir(t), Vector(0,0,1));
212}
213
214
215/**
216  \brief returns the Position of the point calculated on the Curve
217  \return a Vector to the calculated position
218*/
219Vector BezierCurve::getPos(void) const
220{
221  return curvePoint;
222}
223
224
225
226///////////////////////////////////
227//// Uniform Point curve  /////////
228///////////////////////////////////
229/**
230   \brief Creates a new UPointCurve
231*/
232UPointCurve::UPointCurve (void)
233{
234  this->derivation = 0;
235  this->init();
236}
237
238/**
239   \brief Creates a new UPointCurve-Derivation-Curve of deriavation'th degree
240*/
241UPointCurve::UPointCurve (int derivation)
242{
243  this->derivation = derivation;
244  dirCurve=NULL;
245  this->init();
246}
247
248/**
249   \brief Deletes a UPointCurve.
250
[3018]251   It does this by freeing all the space taken over from the nodes
252*/
[3399]253UPointCurve::~UPointCurve(void)
[3018]254{
255  PathNode* tmpNode;
256  currentNode = firstNode;
257  while (tmpNode != 0)
258    {
259      tmpNode = currentNode;
260      currentNode = currentNode->next;
261      delete tmpNode;
262    }
[3399]263  if (dirCurve)
264    delete dirCurve;
[3018]265}
266
267/**
[3399]268   \brief Initializes a UPointCurve
269*/
270void UPointCurve::init(void)
271{
272  nodeCount = 0;
273  firstNode = new PathNode;
274  currentNode = firstNode;
275
276  firstNode->position = Vector (.0, .0, .0);
277  firstNode->number = 0;
278  firstNode->next = 0; // not sure if this really points to NULL!!
279
280  return;
281}
282
283/**
284   \brief Rebuilds a UPointCurve
285   
286   \todo very bad algorithm
287*/
288void UPointCurve::rebuild(void)
289{
290  // rebuilding the Curve itself
291  PathNode* tmpNode = this->firstNode;
292  int i=0;
293  Matrix xTmpMat = Matrix(this->nodeCount, this->nodeCount);
294  Matrix yTmpMat = Matrix(this->nodeCount, this->nodeCount);
295  Matrix zTmpMat = Matrix(this->nodeCount, this->nodeCount);
296  Matrix xValMat = Matrix(this->nodeCount, 3);
297  Matrix yValMat = Matrix(this->nodeCount, 3);
298  Matrix zValMat = Matrix(this->nodeCount, 3);
299  while(tmpNode)
300    {
301      Vector fac = Vector(1,1,1);
302      for (int j = 0; j < this->nodeCount; j++)
303        {
304          xTmpMat(i,j) = fac.x; fac.x *= (float)i/(float)this->nodeCount;//tmpNode->position.x;
305          yTmpMat(i,j) = fac.y; fac.y *= (float)i/(float)this->nodeCount;//tmpNode->position.y;
306          zTmpMat(i,j) = fac.z; fac.z *= (float)i/(float)this->nodeCount;//tmpNode->position.z;
307        }
308      xValMat(i,0) = tmpNode->position.x;
309      yValMat(i,0) = tmpNode->position.y;
310      zValMat(i,0) = tmpNode->position.z;
311      ++i;
312      tmpNode = tmpNode->next;
313    }
314  tmpNode = this->firstNode;
315  xValMat = xTmpMat.Inv() *= xValMat;
316  yValMat = yTmpMat.Inv() *= yValMat;
317  zValMat = zTmpMat.Inv() *= zValMat;
318  i = 0;
319  while(tmpNode)
320    {
321      tmpNode->vFactor.x = xValMat(i,0);
322      tmpNode->vFactor.y = yValMat(i,0);
323      tmpNode->vFactor.z = zValMat(i,0);
324
325      i++;
326      tmpNode = tmpNode->next;
327    }
328}
329
330/**
[3018]331   \brief calculates the Position on the curve
332   \param t The position on the Curve (0<=t<=1)
333   \return the Position on the Path
334*/
[3399]335Vector UPointCurve::calcPos(float t) 
[3018]336{
337  PathNode* tmpNode = firstNode;
338  Vector ret = Vector(0.0,0.0,0.0);
[3399]339  float factor = 1.0;
340  while(tmpNode)
[3018]341    {
[3399]342      ret.x += tmpNode->vFactor.x * factor;
343      ret.y += tmpNode->vFactor.y * factor;
344      ret.z += tmpNode->vFactor.z * factor;
345      factor *= t;
346
[3018]347      tmpNode = tmpNode->next;
348    }
349  return ret;
350}
351
[3217]352/**
353   \brief Calulates the direction of the Curve at time t.
354   \param The time at which to evaluate the curve.
355   \returns The vvaluated Vector.
356*/
[3399]357Vector UPointCurve::calcDir (float t)
[3018]358{
359  PathNode* tmpNode = firstNode;
[3399]360  Vector ret = Vector(0.0,0.0,0.0);
361  float factor = 1.0/t;
362  int k=0;
363  while(tmpNode)
[3018]364    {
[3399]365      ret.x += tmpNode->vFactor.x * factor *k;
366      ret.y += tmpNode->vFactor.y * factor *k;
367      ret.z += tmpNode->vFactor.z * factor *k;
368      factor *= t;
369      k++;
[3018]370      tmpNode = tmpNode->next;
371    }
372  ret.normalize();
[3028]373  return ret;
[3018]374}
375
[3217]376/**
377   \brief Calculates the Quaternion needed for our rotations
378   \param t The time at which to evaluate the cuve.
379   \returns The evaluated Quaternion.
380*/
[3399]381Quaternion UPointCurve::calcQuat (float t)
[3028]382{
383  return Quaternion (calcDir(t), Vector(0,0,1));
384}
385
386
[3018]387/**
388  \brief returns the Position of the point calculated on the Curve
389  \return a Vector to the calculated position
390*/
[3399]391Vector UPointCurve::getPos(void) const
[3018]392{
393  return curvePoint;
394}
Note: See TracBrowser for help on using the repository browser.