source: flair-src/branches/sanscv/lib/FlairCore/src/Vector3D.cpp @ 326

Last change on this file since 326 was 324, checked in by Sanahuja Guillaume, 2 years ago

removing opencv dependency

File size: 7.8 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2013/05/02
6//  filename:   Vector3D.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    Class defining a 3D vector
14//
15//
16/*********************************************************************/
17
18#include "Vector3D.h"
19#include "Vector2D.h"
20#include "RotationMatrix.h"
21#include "Euler.h"
22#include "Quaternion.h"
23#include "Object.h"
24#include <math.h>
25
26namespace flair {
27namespace core {
28 
29template class Vector3D<double>;
30template Vector3D<double> operator+(const Vector3D<double>&, const Vector3D<double>&);
31template Vector3D<double> operator-(const Vector3D<double>&, const Vector3D<double>&);
32template Vector3D<double> operator-(const Vector3D<double>&);
33template Vector3D<double> operator/(const Vector3D<double>&, float);
34template Vector3D<double> operator*(const Vector3D<double>&, const Vector3D<double>&);
35template Vector3D<double> operator*(const Vector3D<double>&, float);
36template Vector3D<double> operator*(float, const Vector3D<double>&);
37template Vector3D<double> CrossProduct(const Vector3D<double>&, const Vector3D<double>&);
38template float DotProduct(const Vector3D<double>&, const Vector3D<double>&);
39
40template class Vector3D<float>;
41template Vector3D<float> operator+(const Vector3D<float>&, const Vector3D<float>&);
42template Vector3D<float> operator-(const Vector3D<float>&, const Vector3D<float>&);
43template Vector3D<float> operator-(const Vector3D<float>&);
44template Vector3D<float> operator/(const Vector3D<float>&, float);
45template Vector3D<float> operator*(const Vector3D<float>&, const Vector3D<float>&);
46template Vector3D<float> operator*(const Vector3D<float>&, float);
47template Vector3D<float> operator*(float, const Vector3D<float>&);
48template Vector3D<float> CrossProduct(const Vector3D<float>&, const Vector3D<float>&);
49template float DotProduct(const Vector3D<float>&, const Vector3D<float>&);
50
51template <typename T> Vector3D<T>::Vector3D(T inX, T inY, T inZ) {
52  x=inX;
53  y=inY;
54  z=inZ;
55}
56
57template <typename T> Vector3D<T>::~Vector3D() {}
58
59/*
60void Vector3D::operator=(const gui::Vector3DSpinBox *vector) {
61    Vector3D vect=vector->Value();
62    x=vect.x;
63    y=vect.y;
64    z=vect.z;
65}*/
66/*
67template<typename T,typename S> Vector3D<T> &Vector3D<T>::operator=(const Vector3D<S> &vector) {
68//template <typename T> Vector3D<T> &Vector3D<T>::operator=(const Vector3D<T> &vector) {
69  x = vector.x;
70  y = vector.y;
71  z = vector.z;
72  return (*this);
73}
74*/
75template <typename T> Vector3D<T> &Vector3D<T>::operator+=(const Vector3D<T> &vector) {
76  x += vector.x;
77  y += vector.y;
78  z += vector.z;
79  return (*this);
80}
81
82template <typename T> Vector3D<T> &Vector3D<T>::operator-=(const Vector3D<T> &vector) {
83  x -= vector.x;
84  y -= vector.y;
85  z -= vector.z;
86  return (*this);
87}
88
89template <typename T> T &Vector3D<T>::operator[](size_t idx) {
90  if (idx == 0) {
91    return x;
92  } else if (idx == 1) {
93    return y;
94  } else if (idx == 2) {
95    return z;
96  } else {
97    Printf("Vector3D: index %i out of bound\n", idx);
98    return z;
99  }
100}
101
102template <typename T> const T &Vector3D<T>::operator[](size_t idx) const {
103  if (idx == 0) {
104    return x;
105  } else if (idx == 1) {
106    return y;
107  } else if (idx == 2) {
108    return z;
109  } else {
110    Printf("Vector3D: index %i out of bound\n", idx);
111    return z;
112  }
113}
114
115template <typename T> Vector3D<T> CrossProduct(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
116  return Vector3D<T>(vectorA.y * vectorB.z - vectorA.z * vectorB.y,
117                  vectorA.z * vectorB.x - vectorA.x * vectorB.z,
118                  vectorA.x * vectorB.y - vectorA.y * vectorB.x);
119}
120
121template <typename T> float DotProduct(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
122  return vectorA.x * vectorB.x + vectorA.y * vectorB.y + vectorA.z * vectorB.z;
123}
124
125template <typename T> Vector3D<T> operator+(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
126  return Vector3D<T>(vectorA.x + vectorB.x, vectorA.y + vectorB.y,
127                  vectorA.z + vectorB.z);
128}
129
130template <typename T> Vector3D<T> operator-(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
131  return Vector3D<T>(vectorA.x - vectorB.x, vectorA.y - vectorB.y,
132                  vectorA.z - vectorB.z);
133}
134
135template <typename T> Vector3D<T> operator-(const Vector3D<T> &vector) {
136  return Vector3D<T>(-vector.x, -vector.y, -vector.z);
137}
138
139template <typename T> Vector3D<T> operator*(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
140  return Vector3D<T>(vectorA.x * vectorB.x, vectorA.y * vectorB.y,
141                  vectorA.z * vectorB.z);
142}
143
144template <typename T> Vector3D<T> operator*(const Vector3D<T> &vector, float coeff) {
145  return Vector3D<T>(vector.x * coeff, vector.y * coeff, vector.z * coeff);
146}
147
148template <typename T> Vector3D<T> operator*(float coeff, const Vector3D<T> &vector) {
149  return Vector3D<T>(vector.x * coeff, vector.y * coeff, vector.z * coeff);
150}
151
152template <typename T> Vector3D<T> operator/(const Vector3D<T> &vector, float coeff) {
153  if (coeff != 0) {
154    return Vector3D<T>(vector.x / coeff, vector.y / coeff, vector.z / coeff);
155  } else {
156    printf("Vector3D: err divinding by 0\n");
157    return Vector3D<T>(0, 0, 0);
158  }
159}
160
161template <typename T> void Vector3D<T>::RotateX(float value) {
162  float y_tmp;
163  y_tmp = y * cosf(value) - z * sinf(value);
164  z = y * sinf(value) + z * cosf(value);
165  y = y_tmp;
166}
167
168template <typename T> void Vector3D<T>::RotateXDeg(float value) { RotateX(Euler::ToRadian(value)); }
169
170template <typename T> void Vector3D<T>::RotateY(float value) {
171  float x_tmp;
172  x_tmp = x * cosf(value) + z * sinf(value);
173  z = -x * sinf(value) + z * cosf(value);
174  x = x_tmp;
175}
176
177template <typename T> void Vector3D<T>::RotateYDeg(float value) { RotateY(Euler::ToRadian(value)); }
178
179template <typename T> void Vector3D<T>::RotateZ(float value) {
180  float x_tmp;
181  x_tmp = x * cosf(value) - y * sinf(value);
182  y = x * sinf(value) + y * cosf(value);
183  x = x_tmp;
184}
185
186template <typename T> void Vector3D<T>::RotateZDeg(float value) { RotateZ(Euler::ToRadian(value)); }
187
188template <typename T> void Vector3D<T>::Rotate(const RotationMatrix &matrix) {
189  T a[3] = {0, 0, 0};
190  T b[3] = {x, y, z};
191
192  for (int i = 0; i < 3; i++) {
193    for (int j = 0; j < 3; j++) {
194      a[i] += matrix.m[i][j] * b[j];
195    }
196  }
197
198  x = a[0];
199  y = a[1];
200  z = a[2];
201}
202
203template <typename T> void Vector3D<T>::Rotate(const Quaternion &quaternion) {
204  RotationMatrix matrix;
205  quaternion.ToRotationMatrix(matrix);
206  Rotate(matrix);
207}
208
209template <typename T> void Vector3D<T>::To2Dxy(Vector2D<T> &vector) const {
210  vector.x = x;
211  vector.y = y;
212}
213
214template <typename T> Vector2D<T> Vector3D<T>::To2Dxy(void) const {
215  Vector2D<T> vect;
216  To2Dxy(vect);
217  return vect;
218}
219
220template <typename T> float Vector3D<T>::GetNorm(void) const { return sqrt(x * x + y * y + z * z); }
221
222template <typename T> void Vector3D<T>::Normalize(void) {
223  float n = GetNorm();
224  if (n != 0) {
225    x = x / n;
226    y = y / n;
227    z = z / n;
228  }
229}
230
231template <typename T> void Vector3D<T>::Saturate(const Vector3D<T> &min, const Vector3D<T> &max) {
232  if (x < min.x)
233    x = min.x;
234  if (x > max.x)
235    x = max.x;
236
237  if (y < min.y)
238    y = min.y;
239  if (y > max.y)
240    y = max.y;
241
242  if (z < min.z)
243    z = min.z;
244  if (z > max.z)
245    z = max.z;
246}
247
248template <typename T> void Vector3D<T>::Saturate(float min, float max) {
249  Saturate(Vector3D<T>(min, min, min), Vector3D<T>(max, max, max));
250}
251
252template <typename T> void Vector3D<T>::Saturate(const Vector3D<T> &value) {
253  float x = fabs(value.x);
254  float y = fabs(value.y);
255  float z = fabs(value.z);
256  Saturate(Vector3D<T>(-x, -y, -z), Vector3D<T>(x, y, z));
257}
258
259template <typename T> void Vector3D<T>::Saturate(float value) {
260  float sat = fabs(value);
261  Saturate(Vector3D<T>(-sat, -sat, -sat), Vector3D<T>(sat, sat, sat));
262}
263
264} // end namespace core
265} // end namespace flair
Note: See TracBrowser for help on using the repository browser.