00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef FCL_VEC_3F_H
00038 #define FCL_VEC_3F_H
00039
00040 #include "fcl/data_types.h"
00041 #include "fcl/math/math_details.h"
00042 #include "fcl/simd/math_simd_details.h"
00043 #include <cmath>
00044 #include <iostream>
00045 #include <limits>
00046
00047
00048 namespace fcl
00049 {
00050
00052 template <typename T>
00053 class Vec3fX
00054 {
00055 public:
00056 typedef typename T::meta_type U;
00057
00059 T data;
00060
00061 Vec3fX() {}
00062 Vec3fX(const Vec3fX& other) : data(other.data) {}
00063
00065 Vec3fX(U x, U y, U z) : data(x, y, z) {}
00066
00068 Vec3fX(U x) : data(x) {}
00069
00071 Vec3fX(const T& data_) : data(data_) {}
00072
00073 inline U operator [] (size_t i) const { return data[i]; }
00074 inline U& operator [] (size_t i) { return data[i]; }
00075
00076 inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
00077 inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
00078 inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
00079 inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
00080 inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
00081 inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
00082 inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
00083 inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
00084 inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
00085 inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
00086 inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
00087 inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
00088 inline Vec3fX& operator += (U t) { data += t; return *this; }
00089 inline Vec3fX& operator -= (U t) { data -= t; return *this; }
00090 inline Vec3fX& operator *= (U t) { data *= t; return *this; }
00091 inline Vec3fX& operator /= (U t) { data /= t; return *this; }
00092 inline Vec3fX operator - () const { return Vec3fX(-data); }
00093 inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
00094 inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
00095 inline Vec3fX& normalize()
00096 {
00097 U sqr_length = details::dot_prod3(data, data);
00098 if(sqr_length > 0)
00099 *this /= (U)sqrt(sqr_length);
00100 return *this;
00101 }
00102
00103 inline Vec3fX& normalize(bool* signal)
00104 {
00105 U sqr_length = details::dot_prod3(data, data);
00106 if(sqr_length > 0)
00107 {
00108 *this /= (U)sqrt(sqr_length);
00109 *signal = true;
00110 }
00111 else
00112 *signal = false;
00113 return *this;
00114 }
00115
00116 inline Vec3fX& abs()
00117 {
00118 data = abs(data);
00119 return *this;
00120 }
00121
00122 inline U length() const { return sqrt(details::dot_prod3(data, data)); }
00123 inline U sqrLength() const { return details::dot_prod3(data, data); }
00124 inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
00125 inline void setValue(U x) { data.setValue(x); }
00126 inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
00127 inline Vec3fX<T>& negate() { data.negate(); return *this; }
00128
00129 inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
00130 {
00131 data.ubound(u.data);
00132 return *this;
00133 }
00134
00135 inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
00136 {
00137 data.lbound(l.data);
00138 return *this;
00139 }
00140
00141 bool isZero() const
00142 {
00143 return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
00144 }
00145
00146 };
00147
00148 template<typename T>
00149 static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
00150 {
00151 typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
00152 if(sqr_length > 0)
00153 return v / (typename T::meta_type)sqrt(sqr_length);
00154 else
00155 return v;
00156 }
00157
00158 template <typename T>
00159 static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
00160 {
00161 return x.dot(y.cross(z));
00162 }
00163
00164 template <typename T>
00165 std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
00166 {
00167 out << x[0] << " " << x[1] << " " << x[2];
00168 return out;
00169 }
00170
00171 template <typename T>
00172 static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
00173 {
00174 return Vec3fX<T>(details::min(x.data, y.data));
00175 }
00176
00177 template <typename T>
00178 static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
00179 {
00180 return Vec3fX<T>(details::max(x.data, y.data));
00181 }
00182
00183 template <typename T>
00184 static inline Vec3fX<T> abs(const Vec3fX<T>& x)
00185 {
00186 return Vec3fX<T>(details::abs(x.data));
00187 }
00188
00189 template <typename T>
00190 void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
00191 {
00192 typedef typename T::meta_type U;
00193 U inv_length;
00194 if(std::abs(w[0]) >= std::abs(w[1]))
00195 {
00196 inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
00197 u[0] = -w[2] * inv_length;
00198 u[1] = (U)0;
00199 u[2] = w[0] * inv_length;
00200 v[0] = w[1] * u[2];
00201 v[1] = w[2] * u[0] - w[0] * u[2];
00202 v[2] = -w[1] * u[0];
00203 }
00204 else
00205 {
00206 inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
00207 u[0] = (U)0;
00208 u[1] = w[2] * inv_length;
00209 u[2] = -w[1] * inv_length;
00210 v[0] = w[1] * u[2] - w[2] * u[1];
00211 v[1] = -w[0] * u[2];
00212 v[2] = w[0] * u[1];
00213 }
00214 }
00215
00216
00217 typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
00218
00219
00220
00221 static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
00222 {
00223 o << "(" << v[0] << " " << v[1] << " " << v[2] << ")";
00224 return o;
00225 }
00226
00227
00228 }
00229
00230
00231 #endif