Ravelin
SpatialArithmetic.h
1 /****************************************************************************
2  * Copyright 2015 Evan Drumwright
3  * This library is distributed under the terms of the Apache V2.0
4  * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0).
5  ****************************************************************************/
6 
8 class SPARITH
9 {
10  public:
11 
13 template <class X>
14 static void from_matrix(const X& m, std::vector<SVELOCITY>& w)
15 {
16  const unsigned SPATIAL_DIM = 6;
17  assert(m.rows() == SPATIAL_DIM);
18  w.resize(m.columns());
19  CONST_COLUMN_ITERATOR data = m.column_iterator_begin();
20  for (unsigned k=0, i=0; i< w.size(); i++, k+= SPATIAL_DIM)
21  w[i] = SVELOCITY(data[k+0], data[k+1], data[k+2], data[k+3], data[k+4], data[k+5]);
22 }
23 
25 template <class X>
26 static X& to_matrix(const std::vector<SVELOCITY>& w, X& m)
27 {
28  const unsigned SPATIAL_DIM = 6;
29  m.resize(SPATIAL_DIM, w.size());
30  COLUMN_ITERATOR data = m.column_iterator_begin();
31  for (unsigned k=0, i=0; i< w.size(); i++)
32  {
33  VECTOR3 f = w[i].get_angular();
34  VECTOR3 t = w[i].get_linear();
35  data[k++] = f[0]; data[k++] = f[1]; data[k++] = f[2];
36  data[k++] = t[0]; data[k++] = t[1]; data[k++] = t[2];
37  }
38 
39  return m;
40 }
41 
43 template <class X>
44 static X& to_matrix(const std::vector<SFORCE>& w, X& m)
45 {
46  const unsigned SPATIAL_DIM = 6;
47  m.resize(SPATIAL_DIM, w.size());
48  COLUMN_ITERATOR data = m.column_iterator_begin();
49  for (unsigned k=0, i=0; i< w.size(); i++)
50  {
51  VECTOR3 f = w[i].get_force();
52  VECTOR3 t = w[i].get_torque();
53  data[k++] = f[0]; data[k++] = f[1]; data[k++] = f[2];
54  data[k++] = t[0]; data[k++] = t[1]; data[k++] = t[2];
55  }
56 
57  return m;
58 }
59 
61 template <class X>
62 static X& to_matrix(const std::vector<SMOMENTUM>& w, X& m)
63 {
64  const unsigned SPATIAL_DIM = 6;
65  m.resize(SPATIAL_DIM, w.size());
66  COLUMN_ITERATOR data = m.column_iterator_begin();
67  for (unsigned k=0, i=0; i< w.size(); i++)
68  {
69  VECTOR3 f = w[i].get_linear();
70  VECTOR3 t = w[i].get_angular();
71  data[k++] = f[0]; data[k++] = f[1]; data[k++] = f[2];
72  data[k++] = t[0]; data[k++] = t[1]; data[k++] = t[2];
73  }
74 
75  return m;
76 }
77 
79 template <class X>
80 static X& spatial_transpose_to_matrix(const std::vector<SMOMENTUM>& w, X& m)
81 {
82  const unsigned SPATIAL_DIM = 6;
83  m.resize(w.size(), SPATIAL_DIM);
84  ROW_ITERATOR data = m.row_iterator_begin();
85  for (unsigned k=0, i=0; i< w.size(); i++)
86  {
87  VECTOR3 f = w[i].get_linear();
88  VECTOR3 t = w[i].get_angular();
89  data[k++] = t[0]; data[k++] = t[1]; data[k++] = t[2];
90  data[k++] = f[0]; data[k++] = f[1]; data[k++] = f[2];
91  }
92 
93  return m;
94 }
95 
97 template <class X>
98 static X& transpose_to_matrix(const std::vector<SVELOCITY>& t, X& m)
99 {
100  const unsigned SPATIAL_DIM = 6;
101  m.resize(t.size(), SPATIAL_DIM);
102  for (unsigned i=0; i< t.size(); i++)
103  {
104  COLUMN_ITERATOR data = m.block_column_iterator_begin(i, i+1, 0, SPATIAL_DIM);
105  VECTOR3 lin = t[i].get_linear();
106  VECTOR3 ang = t[i].get_angular();
107  data[0] = lin[0];
108  data[1] = lin[1];
109  data[2] = lin[2];
110  data[3] = ang[0];
111  data[4] = ang[1];
112  data[5] = ang[2];
113  }
114 
115  return m;
116 }
117 
119 template <class X>
120 static X& transpose_mult(const std::vector<SVELOCITY>& t, const std::vector<SFORCE>& w, X& result)
121 {
122  result.resize(t.size(), w.size());
123  COLUMN_ITERATOR data = result.column_iterator_begin();
124  for (unsigned i=0, k=0; i< t.size(); i++)
125  for (unsigned j=0; j< w.size(); j++)
126  data[k++] = t[i].dot(w[j]);
127 
128  return result;
129 }
130 
132 template <class X>
133 static X& transpose_mult(const std::vector<SVELOCITY>& t, const SFORCE& w, X& result)
134 {
135  result.resize(t.size(), 1, false);
136  COLUMN_ITERATOR data = result.column_iterator_begin();
137  for (unsigned i=0, k=0; i< t.size(); i++)
138  data[k++] = t[i].dot(w);
139 
140  return result;
141 }
142 
144 template <class X>
145 static X& transpose_mult(const std::vector<SVELOCITY>& t, const SMOMENTUM& w, X& result)
146 {
147  result.resize(t.size(), 1, false);
148  COLUMN_ITERATOR data = result.column_iterator_begin();
149  for (unsigned i=0, k=0; i< t.size(); i++)
150  data[k++] = t[i].dot(w);
151 
152  return result;
153 }
154 
156 template <class X>
157 static X& transpose_mult(const std::vector<SVELOCITY>& t, const std::vector<SMOMENTUM>& w, X& result)
158 {
159  result.resize(t.size(), w.size());
160  COLUMN_ITERATOR data = result.column_iterator_begin();
161  for (unsigned i=0, k=0; i< t.size(); i++)
162  for (unsigned j=0; j< w.size(); j++)
163  data[k++] = t[i].dot(w[j]);
164 
165  return result;
166 }
167 
169 template <class Y, class X>
170 static X& transpose_mult(const std::vector<SVELOCITY>& t, const Y& y, X& result)
171 {
172  const unsigned SPATIAL_DIM = 6;
173  result.resize(t.size(), y.columns(), false);
174  COLUMN_ITERATOR data = result.column_iterator_begin();
175  for (unsigned i=0, k=0; i< t.size(); i++)
176  for (unsigned j=0; j< y.columns(); j++)
177  data[k++] = t[i].dot(y.column(j));
178 
179  return result;
180 }
181 
183 template <class X>
184 static X& transpose_mult(const std::vector<SMOMENTUM>& w, const SVELOCITY& t, X& result)
185 {
186  result.resize(w.size());
187  COLUMN_ITERATOR data = result.column_iterator_begin();
188  for (unsigned i=0; i< w.size(); i++)
189  data[i] = w[i].dot(t);
190 
191  return result;
192 }
193 
194  static MATRIXN& mult(const std::vector<SMOMENTUM>& Is, const MATRIXN& m, MATRIXN& result);
195  static VECTORN& mult(const std::vector<SMOMENTUM>& Is, const VECTORN& v, VECTORN& result);
196  static std::vector<SMOMENTUM>& mult(const SPATIAL_AB_INERTIA& I, const std::vector<SVELOCITY>& s, std::vector<SMOMENTUM>& result);
197  static MATRIXN& mult(const SPATIAL_AB_INERTIA& I, const std::vector<SVELOCITY>& s, MATRIXN& result);
198  static std::vector<SMOMENTUM>& mult(const SPATIAL_RB_INERTIA& I, const std::vector<SVELOCITY>& s, std::vector<SMOMENTUM>& result);
199  static MATRIXN& mult(const SPATIAL_RB_INERTIA& I, const std::vector<SVELOCITY>& s, MATRIXN& result);
200  static VECTORN& concat(const VECTORN& v, const SFORCE& w, VECTORN& result);
201  static VECTORN& concat(const VECTORN& v, const SMOMENTUM& w, VECTORN& result);
202  static SVELOCITY mult(const std::vector<SVELOCITY>& a, const VECTORN& v);
203  static SACCEL transform_accel(boost::shared_ptr<const POSE3> target, const SACCEL& a);
204  static std::vector<SACCEL>& transform_accel(boost::shared_ptr<const POSE3> target, const std::vector<SACCEL>& asrc, std::vector<SACCEL>& atgt);
205  static void transform_accel(boost::shared_ptr<const POSE3> target, const SACCEL& w, const VECTOR3& r, const MATRIX3& E, SACCEL& result);
206 
207 }; // end class
208 
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SVELOCITY > &t, const SMOMENTUM &w, X &result)
Computes the "spatial dot product" between a vector of velocities and a momentum and returns the resu...
Definition: SpatialArithmetic.h:145
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SMOMENTUM > &w, const SVELOCITY &t, X &result)
Computes the "spatial dot product" between a vector of momenta and an axis and returns the result in ...
Definition: SpatialArithmetic.h:184
SMOMENTUM
A spatial (six dimensional) momentum.
Definition: SMomentum.h:12
SPARITH::concat
static VECTORN & concat(const VECTORN &v, const SFORCE &w, VECTORN &result)
Concates a vector with a force to make a new vector.
Definition: SpatialArithmetic.cpp:127
SPARITH::transpose_to_matrix
static X & transpose_to_matrix(const std::vector< SVELOCITY > &t, X &m)
Converts an STL vector of spatial velocities to a force matrix (type X)
Definition: SpatialArithmetic.h:98
SPARITH::spatial_transpose_to_matrix
static X & spatial_transpose_to_matrix(const std::vector< SMOMENTUM > &w, X &m)
Converts an STL vector of momenta to a matrix (type X)
Definition: SpatialArithmetic.h:80
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SVELOCITY > &t, const Y &y, X &result)
Computes the "spatial dot product" between a vector of axes and a matrix or vector and returns the re...
Definition: SpatialArithmetic.h:170
MATRIXN
A generic, possibly non-square matrix.
Definition: MatrixN.h:18
SVELOCITY
A spatial velocity (a twist)
Definition: SVelocity.h:15
SPARITH::to_matrix
static X & to_matrix(const std::vector< SVELOCITY > &w, X &m)
Converts an STL vector of axes to a matrix (type X)
Definition: SpatialArithmetic.h:26
SPARITH::from_matrix
static void from_matrix(const X &m, std::vector< SVELOCITY > &w)
Converts a matrix (type X) to a vector of spatial axes.
Definition: SpatialArithmetic.h:14
SPATIAL_AB_INERTIA
A 6x6 spatial algebra matrix typically used for dynamics calculations.
Definition: SpatialABInertia.h:20
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SVELOCITY > &t, const SFORCE &w, X &result)
Computes the "spatial dot product" between a vector of velocities and a force and returns the result ...
Definition: SpatialArithmetic.h:133
SPATIAL_RB_INERTIA
A 6x6 spatial algebra matrix used for dynamics calculations.
Definition: SpatialRBInertia.h:25
SACCEL
A spatial (six dimensional) acceleration.
Definition: SAccel.h:14
VECTORN
A generic N-dimensional floating point vector.
Definition: VectorN.h:16
SPARITH::to_matrix
static X & to_matrix(const std::vector< SMOMENTUM > &w, X &m)
Converts an STL vector of momenta to a matrix (type X)
Definition: SpatialArithmetic.h:62
VECTOR3
A three-dimensional floating point vector used for representing points and vectors in 3D with associa...
Definition: Vector3.h:15
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SVELOCITY > &t, const std::vector< SFORCE > &w, X &result)
Computes the "spatial dot product" between a vector of velocities and a vector of forces and returns ...
Definition: SpatialArithmetic.h:120
SFORCE
A spatial force (a wrench)
Definition: SForce.h:14
COLUMN_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: ColumnIterator.h:12
SPARITH
Class for spatial arithmetic.
Definition: SpatialArithmetic.h:8
CONST_COLUMN_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: ColumnIterator.h:215
MATRIX3
A 3x3 matrix that may be used for orientation, inertia tensors, etc.
Definition: Matrix3.h:20
ROW_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: RowIterator.h:12
SPARITH::transpose_mult
static X & transpose_mult(const std::vector< SVELOCITY > &t, const std::vector< SMOMENTUM > &w, X &result)
Computes the "spatial dot product" between a vector of axes and a vector of momenta and returns the r...
Definition: SpatialArithmetic.h:157
SPARITH::transform_accel
static SACCEL transform_accel(boost::shared_ptr< const POSE3 > target, const SACCEL &a)
Special transformation of acceleration when moving aspect of pose accounted for elsewhere.
Definition: SpatialArithmetic.cpp:45
SPARITH::to_matrix
static X & to_matrix(const std::vector< SFORCE > &w, X &m)
Converts an STL vector of forces to a matrix (type X)
Definition: SpatialArithmetic.h:44
Generated on Wed Mar 9 2016 11:34:02 for Ravelin by   doxygen 1.8.6