1 /* 2 Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien 3 4 This software is provided 'as-is', without any express or implied warranty. 5 In no event will the authors be held liable for any damages arising from the use of this software. 6 Permission is granted to anyone to use this software for any purpose, 7 including commercial applications, and to alter it and redistribute it freely, 8 subject to the following restrictions: 9 10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 3. This notice may not be removed or altered from any source distribution. 13 */ 14 15 ///These spatial algebra classes are used for btMultiBody, 16 ///see BulletDynamics/Featherstone 17 18 #ifndef BT_SPATIAL_ALGEBRA_H 19 #define BT_SPATIAL_ALGEBRA_H 20 21 22 #include "btMatrix3x3.h" 23 24 struct btSpatialForceVector 25 { 26 btVector3 m_topVec, m_bottomVec; 27 // 28 btSpatialForceVector() { setZero(); } 29 btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {} 30 btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) 31 { 32 setValue(ax, ay, az, lx, ly, lz); 33 } 34 // 35 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; } 36 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) 37 { 38 m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz); 39 } 40 // 41 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } 42 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) 43 { 44 m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az; 45 m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz; 46 } 47 // 48 const btVector3 & getLinear() const { return m_topVec; } 49 const btVector3 & getAngular() const { return m_bottomVec; } 50 // 51 void setLinear(const btVector3 &linear) { m_topVec = linear; } 52 void setAngular(const btVector3 &angular) { m_bottomVec = angular; } 53 // 54 void addAngular(const btVector3 &angular) { m_bottomVec += angular; } 55 void addLinear(const btVector3 &linear) { m_topVec += linear; } 56 // 57 void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } 58 // 59 btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } 60 btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } 61 btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); } 62 btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); } 63 btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); } 64 btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); } 65 //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; } 66 }; 67 68 struct btSpatialMotionVector 69 { 70 btVector3 m_topVec, m_bottomVec; 71 // 72 btSpatialMotionVector() { setZero(); } 73 btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {} 74 // 75 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; } 76 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) 77 { 78 m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz); 79 } 80 // 81 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; } 82 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz) 83 { 84 m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az; 85 m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz; 86 } 87 // 88 const btVector3 & getAngular() const { return m_topVec; } 89 const btVector3 & getLinear() const { return m_bottomVec; } 90 // 91 void setAngular(const btVector3 &angular) { m_topVec = angular; } 92 void setLinear(const btVector3 &linear) { m_bottomVec = linear; } 93 // 94 void addAngular(const btVector3 &angular) { m_topVec += angular; } 95 void addLinear(const btVector3 &linear) { m_bottomVec += linear; } 96 // 97 void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); } 98 // 99 btScalar dot(const btSpatialForceVector &b) const 100 { 101 return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec); 102 } 103 // 104 template<typename SpatialVectorType> 105 void cross(const SpatialVectorType &b, SpatialVectorType &out) const 106 { 107 out.m_topVec = m_topVec.cross(b.m_topVec); 108 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); 109 } 110 template<typename SpatialVectorType> 111 SpatialVectorType cross(const SpatialVectorType &b) const 112 { 113 SpatialVectorType out; 114 out.m_topVec = m_topVec.cross(b.m_topVec); 115 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec); 116 return out; 117 } 118 // 119 btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; } 120 btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; } 121 btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; } 122 btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); } 123 btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); } 124 btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); } 125 btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); } 126 }; 127 128 struct btSymmetricSpatialDyad 129 { 130 btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat; 131 // 132 btSymmetricSpatialDyad() { setIdentity(); } 133 btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); } 134 // 135 void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) 136 { 137 m_topLeftMat = topLeftMat; 138 m_topRightMat = topRightMat; 139 m_bottomLeftMat = bottomLeftMat; 140 } 141 // 142 void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) 143 { 144 m_topLeftMat += topLeftMat; 145 m_topRightMat += topRightMat; 146 m_bottomLeftMat += bottomLeftMat; 147 } 148 // 149 void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); } 150 // 151 btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat) 152 { 153 m_topLeftMat -= mat.m_topLeftMat; 154 m_topRightMat -= mat.m_topRightMat; 155 m_bottomLeftMat -= mat.m_bottomLeftMat; 156 return *this; 157 } 158 // 159 btSpatialForceVector operator * (const btSpatialMotionVector &vec) 160 { 161 return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec); 162 } 163 }; 164 165 struct btSpatialTransformationMatrix 166 { 167 btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat; 168 btVector3 m_trnVec; 169 // 170 enum eOutputOperation 171 { 172 None = 0, 173 Add = 1, 174 Subtract = 2 175 }; 176 // 177 template<typename SpatialVectorType> 178 void transform( const SpatialVectorType &inVec, 179 SpatialVectorType &outVec, 180 eOutputOperation outOp = None) 181 { 182 if(outOp == None) 183 { 184 outVec.m_topVec = m_rotMat * inVec.m_topVec; 185 outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; 186 } 187 else if(outOp == Add) 188 { 189 outVec.m_topVec += m_rotMat * inVec.m_topVec; 190 outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; 191 } 192 else if(outOp == Subtract) 193 { 194 outVec.m_topVec -= m_rotMat * inVec.m_topVec; 195 outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec; 196 } 197 198 } 199 200 template<typename SpatialVectorType> 201 void transformRotationOnly( const SpatialVectorType &inVec, 202 SpatialVectorType &outVec, 203 eOutputOperation outOp = None) 204 { 205 if(outOp == None) 206 { 207 outVec.m_topVec = m_rotMat * inVec.m_topVec; 208 outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec; 209 } 210 else if(outOp == Add) 211 { 212 outVec.m_topVec += m_rotMat * inVec.m_topVec; 213 outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec; 214 } 215 else if(outOp == Subtract) 216 { 217 outVec.m_topVec -= m_rotMat * inVec.m_topVec; 218 outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec; 219 } 220 221 } 222 223 template<typename SpatialVectorType> 224 void transformInverse( const SpatialVectorType &inVec, 225 SpatialVectorType &outVec, 226 eOutputOperation outOp = None) 227 { 228 if(outOp == None) 229 { 230 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; 231 outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); 232 } 233 else if(outOp == Add) 234 { 235 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; 236 outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); 237 } 238 else if(outOp == Subtract) 239 { 240 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; 241 outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec)); 242 } 243 } 244 245 template<typename SpatialVectorType> 246 void transformInverseRotationOnly( const SpatialVectorType &inVec, 247 SpatialVectorType &outVec, 248 eOutputOperation outOp = None) 249 { 250 if(outOp == None) 251 { 252 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec; 253 outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec; 254 } 255 else if(outOp == Add) 256 { 257 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec; 258 outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec; 259 } 260 else if(outOp == Subtract) 261 { 262 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec; 263 outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec; 264 } 265 266 } 267 268 void transformInverse( const btSymmetricSpatialDyad &inMat, 269 btSymmetricSpatialDyad &outMat, 270 eOutputOperation outOp = None) 271 { 272 const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1], 273 m_trnVec[2], 0, -m_trnVec[0], 274 -m_trnVec[1], m_trnVec[0], 0); 275 276 277 if(outOp == None) 278 { 279 outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; 280 outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; 281 outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; 282 } 283 else if(outOp == Add) 284 { 285 outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; 286 outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; 287 outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; 288 } 289 else if(outOp == Subtract) 290 { 291 outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat; 292 outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat; 293 outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat; 294 } 295 } 296 297 template<typename SpatialVectorType> 298 SpatialVectorType operator * (const SpatialVectorType &vec) 299 { 300 SpatialVectorType out; 301 transform(vec, out); 302 return out; 303 } 304 }; 305 306 template<typename SpatialVectorType> 307 void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out) 308 { 309 //output op maybe? 310 311 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); 312 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); 313 out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); 314 //maybe simple a*spatTranspose(a) would be nicer? 315 } 316 317 template<typename SpatialVectorType> 318 btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b) 319 { 320 btSymmetricSpatialDyad out; 321 322 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec); 323 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec); 324 out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec); 325 326 return out; 327 //maybe simple a*spatTranspose(a) would be nicer? 328 } 329 330 #endif //BT_SPATIAL_ALGEBRA_H 331 332
