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 | //#include "Vector3DSpinBox.h"
|
---|
26 |
|
---|
27 | namespace flair { namespace core {
|
---|
28 |
|
---|
29 | Vector3D::Vector3D(float inX,float inY,float inZ): x(inX),y(inY),z(inZ){
|
---|
30 | }
|
---|
31 |
|
---|
32 | Vector3D::~Vector3D() {
|
---|
33 |
|
---|
34 | }
|
---|
35 | /*
|
---|
36 | void Vector3D::operator=(const gui::Vector3DSpinBox *vector) {
|
---|
37 | Vector3D vect=vector->Value();
|
---|
38 | x=vect.x;
|
---|
39 | y=vect.y;
|
---|
40 | z=vect.z;
|
---|
41 | }*/
|
---|
42 |
|
---|
43 | Vector3D &Vector3D::operator=(const Vector3D &vector) {
|
---|
44 | x=vector.x;
|
---|
45 | y=vector.y;
|
---|
46 | z=vector.z;
|
---|
47 | return (*this);
|
---|
48 | }
|
---|
49 |
|
---|
50 | Vector3D &Vector3D::operator+=(const Vector3D &vector) {
|
---|
51 | x+=vector.x;
|
---|
52 | y+=vector.y;
|
---|
53 | z+=vector.z;
|
---|
54 | return (*this);
|
---|
55 | }
|
---|
56 |
|
---|
57 | Vector3D &Vector3D::operator-=(const Vector3D &vector) {
|
---|
58 | x-=vector.x;
|
---|
59 | y-=vector.y;
|
---|
60 | z-=vector.z;
|
---|
61 | return (*this);
|
---|
62 | }
|
---|
63 |
|
---|
64 | float &Vector3D::operator[](size_t idx) {
|
---|
65 | if(idx==0) {
|
---|
66 | return x;
|
---|
67 | } else if(idx==1) {
|
---|
68 | return y;
|
---|
69 | } else if(idx==2) {
|
---|
70 | return z;
|
---|
71 | } else {
|
---|
72 | Printf("Vector3D: index %i out of bound\n",idx);
|
---|
73 | return z;
|
---|
74 | }
|
---|
75 | }
|
---|
76 |
|
---|
77 | const float &Vector3D::operator[](size_t idx) const {
|
---|
78 | if(idx==0) {
|
---|
79 | return x;
|
---|
80 | } else if(idx==1) {
|
---|
81 | return y;
|
---|
82 | } else if(idx==2) {
|
---|
83 | return z;
|
---|
84 | } else {
|
---|
85 | Printf("Vector3D: index %i out of bound\n",idx);
|
---|
86 | return z;
|
---|
87 | }
|
---|
88 | }
|
---|
89 |
|
---|
90 | Vector3D CrossProduct(const Vector3D &vectorA,const Vector3D &vectorB) {
|
---|
91 | return Vector3D(vectorA.y*vectorB.z - vectorA.z*vectorB.y,
|
---|
92 | vectorA.z*vectorB.x - vectorA.x*vectorB.z,
|
---|
93 | vectorA.x*vectorB.y - vectorA.y*vectorB.x);
|
---|
94 | }
|
---|
95 |
|
---|
96 | float DotProduct(const Vector3D &vectorA,const Vector3D &vectorB) {
|
---|
97 | return vectorA.x*vectorB.x + vectorA.y*vectorB.y+ vectorA.z*vectorB.z;
|
---|
98 | }
|
---|
99 |
|
---|
100 | Vector3D operator+ (const Vector3D &vectorA,const Vector3D &vectorB) {
|
---|
101 | return Vector3D(vectorA.x + vectorB.x,
|
---|
102 | vectorA.y + vectorB.y,
|
---|
103 | vectorA.z + vectorB.z);
|
---|
104 | }
|
---|
105 |
|
---|
106 | Vector3D operator- (const Vector3D &vectorA,const Vector3D &vectorB) {
|
---|
107 | return Vector3D(vectorA.x - vectorB.x,
|
---|
108 | vectorA.y - vectorB.y,
|
---|
109 | vectorA.z - vectorB.z);
|
---|
110 | }
|
---|
111 |
|
---|
112 | Vector3D operator-(const Vector3D &vector) {
|
---|
113 | return Vector3D(-vector.x,-vector.y,-vector.z);
|
---|
114 | }
|
---|
115 |
|
---|
116 | Vector3D operator * (const Vector3D &vectorA,const Vector3D &vectorB) {
|
---|
117 | return Vector3D(vectorA.x* vectorB.x,
|
---|
118 | vectorA.y* vectorB.y,
|
---|
119 | vectorA.z* vectorB.z);
|
---|
120 | }
|
---|
121 |
|
---|
122 | Vector3D operator * (const Vector3D &vector, float coeff) {
|
---|
123 | return Vector3D(vector.x* coeff,
|
---|
124 | vector.y* coeff,
|
---|
125 | vector.z* coeff);
|
---|
126 | }
|
---|
127 |
|
---|
128 | Vector3D operator * (float coeff,const Vector3D &vector) {
|
---|
129 | return Vector3D(vector.x* coeff,
|
---|
130 | vector.y* coeff,
|
---|
131 | vector.z* coeff);
|
---|
132 | }
|
---|
133 |
|
---|
134 | Vector3D operator/ (const Vector3D &vector, float coeff) {
|
---|
135 | if(coeff!=0) {
|
---|
136 | return Vector3D(vector.x/ coeff,
|
---|
137 | vector.y/ coeff,
|
---|
138 | vector.z/ coeff);
|
---|
139 | } else {
|
---|
140 | printf("Vector3D: err divinding by 0\n");
|
---|
141 | return Vector3D(0,0,0);
|
---|
142 | }
|
---|
143 | }
|
---|
144 |
|
---|
145 | void Vector3D::RotateX(float value) {
|
---|
146 | float y_tmp;
|
---|
147 | y_tmp=y*cosf(value)-z*sinf(value);
|
---|
148 | z=y*sinf(value)+z*cosf(value);
|
---|
149 | y=y_tmp;
|
---|
150 | }
|
---|
151 |
|
---|
152 | void Vector3D::RotateXDeg(float value) {
|
---|
153 | RotateX(Euler::ToRadian(value));
|
---|
154 | }
|
---|
155 |
|
---|
156 | void Vector3D::RotateY(float value) {
|
---|
157 | float x_tmp;
|
---|
158 | x_tmp=x*cosf(value)+z*sinf(value);
|
---|
159 | z=-x*sinf(value)+z*cosf(value);
|
---|
160 | x=x_tmp;
|
---|
161 | }
|
---|
162 |
|
---|
163 | void Vector3D::RotateYDeg(float value) {
|
---|
164 | RotateY(Euler::ToRadian(value));
|
---|
165 | }
|
---|
166 |
|
---|
167 | void Vector3D::RotateZ(float value) {
|
---|
168 | float x_tmp;
|
---|
169 | x_tmp=x*cosf(value)-y*sinf(value);
|
---|
170 | y=x*sinf(value)+y*cosf(value);
|
---|
171 | x=x_tmp;
|
---|
172 | }
|
---|
173 |
|
---|
174 | void Vector3D::RotateZDeg(float value) {
|
---|
175 | RotateZ(Euler::ToRadian(value));
|
---|
176 | }
|
---|
177 |
|
---|
178 | void Vector3D::Rotate(const RotationMatrix &matrix) {
|
---|
179 | float a[3]= {0,0,0};
|
---|
180 | float b[3]= {x,y,z};
|
---|
181 |
|
---|
182 | for(int i=0; i<3; i++) {
|
---|
183 | for(int j=0; j<3; j++) {
|
---|
184 | a[i]+=matrix.m[i][j]*b[j];
|
---|
185 | }
|
---|
186 | }
|
---|
187 |
|
---|
188 | x=a[0];
|
---|
189 | y=a[1];
|
---|
190 | z=a[2];
|
---|
191 | }
|
---|
192 |
|
---|
193 | void Vector3D::Rotate(const Quaternion &quaternion) {
|
---|
194 | RotationMatrix matrix;
|
---|
195 | quaternion.ToRotationMatrix(matrix);
|
---|
196 | Rotate(matrix);
|
---|
197 | }
|
---|
198 |
|
---|
199 | void Vector3D::To2Dxy(Vector2D &vector) const {
|
---|
200 | vector.x=x;
|
---|
201 | vector.y=y;
|
---|
202 | }
|
---|
203 |
|
---|
204 | Vector2D Vector3D::To2Dxy(void) const {
|
---|
205 | Vector2D vect;
|
---|
206 | To2Dxy(vect);
|
---|
207 | return vect;
|
---|
208 | }
|
---|
209 |
|
---|
210 | float Vector3D::GetNorm(void) const {
|
---|
211 | return sqrt(x*x+y*y+z*z);
|
---|
212 | }
|
---|
213 |
|
---|
214 | void Vector3D::Normalize(void) {
|
---|
215 | float n=GetNorm();
|
---|
216 | if(n!=0) {
|
---|
217 | x=x/n;
|
---|
218 | y=y/n;
|
---|
219 | z=z/n;
|
---|
220 | }
|
---|
221 | }
|
---|
222 |
|
---|
223 | void Vector3D::Saturate(const Vector3D &min,const Vector3D &max) {
|
---|
224 | if(x<min.x) x=min.x;
|
---|
225 | if(x>max.x) x=max.x;
|
---|
226 |
|
---|
227 | if(y<min.y) y=min.y;
|
---|
228 | if(y>max.y) y=max.y;
|
---|
229 |
|
---|
230 | if(z<min.z) z=min.z;
|
---|
231 | if(z>max.z) z=max.z;
|
---|
232 | }
|
---|
233 |
|
---|
234 | void Vector3D::Saturate(float min,float max) {
|
---|
235 | Saturate(Vector3D(min,min,min),Vector3D(max,max,max));
|
---|
236 | }
|
---|
237 |
|
---|
238 | void Vector3D::Saturate(const Vector3D &value) {
|
---|
239 | float x=fabs(value.x);
|
---|
240 | float y=fabs(value.y);
|
---|
241 | float z=fabs(value.z);
|
---|
242 | Saturate(Vector3D(-x,-y,-z),Vector3D(x,y,z));
|
---|
243 | }
|
---|
244 |
|
---|
245 | void Vector3D::Saturate(float value) {
|
---|
246 | float sat=fabs(value);
|
---|
247 | Saturate(Vector3D(-sat,-sat,-sat),Vector3D(sat,sat,sat));
|
---|
248 | }
|
---|
249 |
|
---|
250 | } // end namespace core
|
---|
251 | } // end namespace flair
|
---|