UnitVec.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007 #ifndef SimTK_UNITVEC_H
00008 #define SimTK_UNITVEC_H
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
00035
00036
00037
00038
00039
00040
00041
00042 #include "SimTKcommon/SmallMatrix.h"
00043
00044 #include <iosfwd>
00045
00046
00047
00048 namespace SimTK {
00049
00050
00051
00052 template <int S> class UnitRow;
00053
00054
00055
00061
00062 template <int S>
00063 class UnitVec : public Vec<3,Real,S> {
00064 public:
00065 typedef Vec<3,Real,S> BaseVec;
00066 typedef UnitRow<S> TransposeType;
00067
00068 UnitVec() : BaseVec(NaN) { }
00069
00070
00071 UnitVec(const UnitVec& u) : BaseVec( static_cast<const BaseVec&>(u) ) {}
00072
00073
00074 template <int S2> UnitVec(const UnitVec<S2>& u) : BaseVec( static_cast<const typename UnitVec<S2>::BaseVec&>(u) ) {}
00075
00076
00077 explicit UnitVec(const BaseVec& v) : BaseVec(v/v.norm()) {}
00078 template <int S2> explicit UnitVec(const Vec<3,Real,S2>& v) : BaseVec( v/v.norm() ) {}
00079
00080 UnitVec(const Real& x, const Real& y, const Real& z) : BaseVec(x,y,z) { static_cast<BaseVec&>(*this) /= BaseVec::norm(); }
00081
00082
00083 explicit UnitVec(int axis) : BaseVec(0) {
00084 assert(0 <= axis && axis <= 2);
00085 BaseVec::operator[](axis) = 1.;
00086 }
00087
00088 UnitVec& operator=(const UnitVec& u) {
00089 BaseVec::operator=(static_cast<const BaseVec&>(u));
00090 return *this;
00091 }
00092 template <int S2> UnitVec& operator=(const UnitVec<S2>& u) {
00093 BaseVec::operator=(static_cast<const typename UnitVec<S2>::BaseVec&>(u));
00094 return *this;
00095 }
00096
00097 const BaseVec& asVec3() const {return static_cast<const BaseVec&>(*this);}
00098
00099
00100
00101 UnitVec<1> negate() const {return UnitVec<1>(-asVec3(),true);}
00102 UnitVec<1> operator-() const {return negate();}
00103
00104 const TransposeType& operator~() const { return *reinterpret_cast<const TransposeType*>(this); }
00105 TransposeType& operator~() { return *reinterpret_cast<TransposeType*>(this); }
00106
00107
00108
00109 const Real& operator[](int i) const { return BaseVec::operator[](i); }
00110 const Real& operator()(int i) const { return BaseVec::operator()(i); }
00111
00112
00113
00114
00115
00116
00117 UnitVec<1> abs() const { return UnitVec<1>( asVec3().abs(), true ); }
00118
00119
00120 inline UnitVec<1> perp() const;
00121
00122
00123
00124 UnitVec( const BaseVec& v, bool ) : BaseVec(v) { }
00125 template <int S2> UnitVec( const Vec<3,Real,S2>& v, bool ) : BaseVec(v) { }
00126 };
00127
00128
00129 template <int S>
00130 inline UnitVec<1> UnitVec<S>::perp() const {
00131
00132
00133 const UnitVec<1> u(abs());
00134 const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
00135 : (u[1] <= u[2] ? 1 : 2);
00136
00137 return UnitVec<1>( *this % UnitVec<1>(minAxis) );
00138 }
00139
00140
00141
00146
00147 template <int S>
00148 class UnitRow : public Row<3,Real,S> {
00149 public:
00150 typedef Row<3,Real,S> BaseRow;
00151 typedef UnitVec<S> TransposeType;
00152
00153 UnitRow() : BaseRow(NaN) { }
00154
00155
00156 UnitRow(const UnitRow& u)
00157 : BaseRow(static_cast<const BaseRow&>(u)) { }
00158
00159
00160 template <int S2> UnitRow(const UnitRow<S2>& u)
00161 : BaseRow(static_cast<const typename UnitRow<S2>::BaseRow&>(u)) { }
00162
00163
00164 UnitRow& operator=(const UnitRow& u) {
00165 BaseRow::operator=(static_cast<const BaseRow&>(u));
00166 return *this;
00167 }
00168
00169 template <int S2> UnitRow& operator=(const UnitRow<S2>& u) {
00170 BaseRow::operator=(static_cast<const typename UnitRow<S2>::BaseRow&>(u));
00171 return *this;
00172 }
00173
00174
00175 explicit UnitRow(const BaseRow& v) : BaseRow(v/v.norm()) { }
00176 template <int S2> explicit UnitRow(const Row<3,Real,S2>& v)
00177 : BaseRow(v/v.norm()) { }
00178
00179 UnitRow(const Real& x, const Real& y, const Real& z) : BaseRow(x,y,z) {
00180 static_cast<BaseRow&>(*this) /= BaseRow::norm();
00181 }
00182
00183
00184 explicit UnitRow(int axis) : BaseRow(0) {
00185 assert(0 <= axis && axis <= 2);
00186 BaseRow::operator[](axis) = 1.;
00187 }
00188
00189 const BaseRow& asRow3() const { return static_cast<const BaseRow&>(*this); }
00190
00191
00192
00193 UnitRow<1> negate() const { return UnitRow<1>(-asRow3(),true); }
00194 UnitRow<1> operator-() const { return negate();}
00195
00196 const TransposeType& operator~() const { return *reinterpret_cast<const TransposeType*>(this); }
00197 TransposeType& operator~() { return *reinterpret_cast<TransposeType*>(this); }
00198
00199
00200
00201 const Real& operator[](int i) const { return BaseRow::operator[](i); }
00202 const Real& operator()(int i) const { return BaseRow::operator()(i); }
00203
00204
00205
00206
00207
00208
00209 UnitRow<1> abs() const { return UnitRow<1>(asRow3().abs(),true); }
00210
00211
00212 inline UnitRow<1> perp() const;
00213
00214
00215
00216 UnitRow( const BaseRow& v, bool ) : BaseRow(v) { }
00217 template <int S2> UnitRow( const Row<3,Real,S2>& v, bool ) : BaseRow(v) { }
00218 };
00219
00220 template <int S>
00221 inline UnitRow<1> UnitRow<S>::perp() const {
00222
00223
00224 const UnitRow<1> u(abs());
00225 const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
00226 : (u[1] <= u[2] ? 1 : 2);
00227
00228 return UnitRow<1>(*this % UnitRow<1>(minAxis));
00229 }
00230
00231
00232
00233 typedef UnitVec<1> UnitVec3;
00234
00235
00236
00237 }
00238
00239
00240 #endif // SimTK_UNITVEC_H_
00241
00242
00243