-
Notifications
You must be signed in to change notification settings - Fork 299
/
SpatialVector.h
158 lines (142 loc) · 5.24 KB
/
SpatialVector.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
/******************************************************************************
* SOFA, Simulation Open-Framework Architecture *
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: contact@sofa-framework.org *
******************************************************************************/
#pragma once
#include <sofa/type/Mat.h>
#include <sofa/type/config.h>
namespace sofa::type
{
/**
* A spatial vector.
* When representing a velocity, lineVec is the angular velocity and freeVec is the linear velocity.
* When representing a spatial force, lineVec is the force and freeVec is the torque.
* */
template<class TReal>
class SpatialVector
{
public:
using Real = TReal;
using Vec = sofa::type::Vec<3, TReal>;
using Mat66 = sofa::type::Mat<6, 6, TReal>;
Vec lineVec{ type::NOINIT };
Vec freeVec{ type::NOINIT };
void clear();
SpatialVector() = default;
/**
\param l The line vector: angular velocity, or force
\param f The free vector: linear velocity, or torque
*/
SpatialVector( const Vec& l, const Vec& f );
SpatialVector& operator+= (const SpatialVector& v);
//template<class Real2>
SpatialVector operator* ( Real a ) const
{
return SpatialVector( lineVec *a, freeVec * a);
}
SpatialVector& operator*= ( Real a )
{
lineVec *=a;
freeVec *= a;
return *this;
}
SpatialVector operator + ( const SpatialVector& v ) const;
SpatialVector operator - ( const SpatialVector& v ) const;
SpatialVector operator - ( ) const;
/// Spatial dot product (cross terms)
Real operator* ( const SpatialVector& v ) const;
/// Spatial cross product
SpatialVector cross( const SpatialVector& v ) const;
/// product with a dense matrix
SpatialVector operator* (const Mat66&) const;
/// write to an output stream
inline friend std::ostream& operator << (std::ostream& out, const SpatialVector& t )
{
out << t.lineVec << " " << t.freeVec;
return out;
}
/// read from an input stream
inline friend std::istream& operator >> ( std::istream& in, SpatialVector& t )
{
in >> t.lineVec >> t.freeVec;
return in;
}
/// If the SpatialVector models a spatial velocity, then the linear velocity is the freeVec.
/// Otherwise, the SpatialVector models a spatial force, and this method returns a torque.
Vec& getLinearVelocity()
{
return freeVec;
}
const Vec& getLinearVelocity() const
{
return freeVec;
}
void setLinearVelocity(const Vec& v)
{
freeVec = v;
}
/// If the SpatialVector models a spatial velocity, then the angular velocity is the lineVec.
/// Otherwise, the SpatialVector models a spatial force, and this method returns a force.
Vec& getAngularVelocity()
{
return lineVec;
}
const Vec& getAngularVelocity() const
{
return lineVec;
}
void setAngularVelocity(const Vec& v)
{
lineVec = v;
}
/// If the SpatialVector models a spatial force, then the torque is the freeVec.
/// Otherwise, the SpatialVector models a spatial velocity, and this method returns a linear velocity.
Vec& getTorque()
{
return freeVec;
}
const Vec& getTorque() const
{
return freeVec;
}
void setTorque(const Vec& v)
{
freeVec = v;
}
/// If the SpatialVector models a spatial force, then the torque is the lineVec.
/// Otherwise, the SpatialVector models a spatial velocity, and this method returns an angular velocity.
Vec& getForce()
{
return lineVec;
}
const Vec& getForce() const
{
return lineVec;
}
void setForce(const Vec& v)
{
lineVec = v;
}
};
#if !defined(SOFA_TYPE_SPATIALVECTOR_CPP)
extern template class SOFA_TYPE_API SpatialVector<double>;
extern template class SOFA_TYPE_API SpatialVector<float>;
#endif
}