Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/basic.h

raw
1 #ifndef _BASIC_H_
2 #define _BASIC_H_
3 #ifndef _USE_MATH_DEFINES
4 #define _USE_MATH_DEFINES
5 #endif
6 #include <cmath>
7 #include <cassert>
8 #include <GfxTL/VectorXD.h>
9
10 #ifndef DLL_LINKAGE
11 #define DLL_LINKAGE
12 #endif
13
14 typedef float Scalar;
15
16 class DLL_LINKAGE Vec3f {
17
18 public:
19 typedef float ScalarType;
20 enum { Dim = 3 };
21
22 Vec3f () {}
23 Vec3f (float x, float y, float z) { setValue(x, y, z); }
24
25 Vec3f(const GfxTL::Vector3Df &v)
26 {
27 for(unsigned int i = 0; i < 3; ++i)
28 vec[i] = v[i];
29 }
30
31 explicit Vec3f (const float v[3])
32 {
33 setValue(v);
34 }
35
36 Vec3f& setValue (const float v[3])
37 {
38 for (int i = 0; i < 3; i++)
39 vec[i] = v[i];
40 return *this;
41 }
42
43 const float* getValue () const
44 {
45 return vec;
46 }
47
48 Vec3f& setValue (float x, float y, float z)
49 {
50 vec[0] = x; vec[1] = y; vec[2] = z;
51 return *this;
52 }
53
54 void getValue (float& x, float& y, float& z) const
55 {
56 x = vec[0]; y = vec[1]; z = vec[2];
57 }
58
59
60 operator const float* () const
61 {
62 return vec;
63 }
64
65 operator float *()
66 {
67 return vec;
68 }
69
70 /*float& operator [] (int i)
71 {
72 return vec[i];
73 }
74
75 const float& operator [] (int i) const
76 {
77 return vec[i];
78 }*/
79
80 float dot (const Vec3f& v) const
81 {
82 float s = vec[0] * v.vec[0];
83 for (int i = 1; i < 3; i++)
84 s += vec[i] * v.vec[i];
85 return s;
86 }
87
88 float dot(const float *v) const
89 {
90 return vec[0] * v[0] + vec[1] * v[1] + vec[2] * v[2];
91 }
92
93 Vec3f cross( const Vec3f& v ) const
94 {
95 return Vec3f(vec[1] * v.vec[2] - vec[2] * v.vec[1],
96 vec[2] * v.vec[0] - vec[0] * v.vec[2],
97 vec[0] * v.vec[1] - vec[1] * v.vec[0]);
98 }
99
100
101 float sqrLength () const
102 {
103 return dot(*this);
104 }
105
106 float length () const
107 {
108 return (float) std::sqrt(dot(*this));
109 }
110
111 float normalize ()
112 {
113 float len = length();
114 if (len > 0)
115 *this /= len;
116 return len;
117 }
118
119 bool equals (const Vec3f& v, float tolerance) const
120 {
121 return ((*this - v).sqrLength() <= tolerance);
122 }
123
124 friend bool operator == (const Vec3f& v1, const Vec3f& v2)
125 {
126 for (int i = 0; i < 3; i++)
127 if (v1.vec[i] != v2.vec[i])
128 return false;
129 return true;
130 }
131
132 friend bool operator < (const Vec3f& v1, const Vec3f& v2)
133 {
134 for (int i = 0; i < 3; i++)
135 if (v1.vec[i] >= v2.vec[i])
136 return false;
137 return true;
138 }
139 friend bool operator <= (const Vec3f& v1, const Vec3f& v2)
140 {
141 for (int i = 0; i < 3; i++)
142 if (v1.vec[i] > v2.vec[i])
143 return false;
144 return true;
145 }
146 friend bool operator > (const Vec3f& v1, const Vec3f& v2)
147 {
148 for (int i = 0; i < 3; i++)
149 if (v1.vec[i] <= v2.vec[i])
150 return false;
151 return true;
152 }
153 friend bool operator >= (const Vec3f& v1, const Vec3f& v2)
154 {
155 for (int i = 0; i < 3; i++)
156 if (v1.vec[i] < v2.vec[i])
157 return false;
158 return true;
159 }
160
161 friend bool operator != (const Vec3f& v1, const Vec3f& v2)
162 {
163 return !(v1 == v2);
164 }
165
166 Vec3f& operator *= (float s)
167 {
168 for (int i = 0; i < 3; i++)
169 vec[i] *= s;
170 return *this;
171 }
172
173 Vec3f& operator /= (float s)
174 {
175 for (int i = 0; i < 3; i++)
176 vec[i] /= s;
177 return *this;
178 }
179
180 Vec3f& operator += (const Vec3f& v)
181 {
182 for (int i = 0; i < 3; i++)
183 vec[i] += v.vec[i];
184 return *this;
185 }
186
187 Vec3f& operator -= (const Vec3f& v)
188 {
189 for (int i = 0; i < 3; i++)
190 vec[i] -= v.vec[i];
191 return *this;
192 }
193
194 void negate ()
195 {
196 for (int i = 0; i < 3; i++)
197 vec[i] = -vec[i];
198 }
199
200
201 friend Vec3f operator * (float s, const Vec3f& v) { return Vec3f(v) *= s; }
202 friend Vec3f operator * (const Vec3f& v, float s) { return Vec3f(v) *= s; }
203 friend Vec3f operator / (const Vec3f& v, float s) { return Vec3f(v) /= s; }
204 friend Vec3f operator + (const Vec3f& v1, const Vec3f& v2) { return Vec3f(v1) += v2; }
205 friend Vec3f operator - (const Vec3f& v1, const Vec3f& v2) { return Vec3f(v1) -= v2; }
206 friend Vec3f operator - (const Vec3f& v1) { Vec3f v2 = v1; v2.negate(); return v2; }
207 Vec3f operator*(const Vec3f &v) const { return Vec3f(vec[0] * v.vec[0], vec[1] * v.vec[1], vec[2] * v.vec[2]); }
208
209 // Konvertierung:
210
211 Vec3f& setValue (const Vec3f& v)
212 {
213 for (int i = 0; i < 3; i++)
214 vec[i] = (float) v[i];
215 return *this;
216 }
217
218 void getValue (Vec3f& v) const
219 {
220 for (int i = 0; i < 3; i++)
221 v[i] = (Scalar) vec[i];
222 }
223
224 private:
225 float vec[3];
226 };
227
228 #endif //_BASIC_H_