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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
|
/**
* $Id$
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under
* the Blender License. See http://www.blender.org/BL/ for information
* about this.
*
* 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Original author: Laurence
* Contributor(s): Brecht
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
#ifndef NAN_INCLUDED_IK_QSegment_h
#define NAN_INCLUDED_IK_QSegment_h
#include "MT_Vector3.h"
#include "MT_Transform.h"
#include "MT_Matrix4x4.h"
#include "IK_QJacobian.h"
#include "MEM_SmartPtr.h"
#include <vector>
/**
* An IK_Qsegment encodes information about a segments
* local coordinate system.
*
* These segments always point along the y-axis.
*
* Here we define the local coordinates of a joint as
* local_transform =
* translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
* We use the standard moto column ordered matrices. You can read
* this as:
* - first translate by (0,length,0)
* - multiply by the rotation matrix derived from the current
* angle parameterization q.
* - multiply by the user defined matrix representing the rest
* position of the bone.
* - translate by the used defined translation (tr1)
* The ordering of these transformations is vital, you must
* use exactly the same transformations when displaying the segments
*/
class IK_QSegment
{
public:
virtual ~IK_QSegment();
// start: a user defined translation
// rest_basis: a user defined rotation
// basis: a user defined rotation
// length: length of this segment
void SetTransform(
const MT_Vector3& start,
const MT_Matrix3x3& rest_basis,
const MT_Matrix3x3& basis,
const MT_Scalar length
);
// tree structure access
void SetParent(IK_QSegment *parent);
IK_QSegment *Child() const
{ return m_child; }
IK_QSegment *Sibling() const
{ return m_sibling; }
IK_QSegment *Parent() const
{ return m_parent; }
// for combining two joints into one from the interface
void SetComposite(IK_QSegment *seg);
IK_QSegment *Composite() const
{ return m_composite; }
// number of degrees of freedom
int NumberOfDoF() const
{ return m_num_DoF; }
// unique id for this segment, for identification in the jacobian
int DoFId() const
{ return m_DoF_id; }
void SetDoFId(int dof_id)
{ m_DoF_id = dof_id; }
// the max distance of the end of this bone from the local origin.
const MT_Scalar MaxExtension() const
{ return m_max_extension; }
// the change in rotation and translation w.r.t. the rest pose
MT_Matrix3x3 BasisChange() const;
MT_Vector3 TranslationChange() const;
// the start and end of the segment
const MT_Point3 &GlobalStart() const
{ return m_global_start; }
const MT_Point3 &GlobalEnd() const
{ return m_global_transform.getOrigin(); }
// the global transformation at the end of the segment
const MT_Transform &GlobalTransform() const
{ return m_global_transform; }
// is a translational segment?
bool Translational() const
{ return m_translational; }
// locking (during inner clamping loop)
bool Locked(int dof) const
{ return m_locked[dof]; }
void UnLock()
{ m_locked[0] = m_locked[1] = m_locked[2] = false; }
// per dof joint weighting
MT_Scalar Weight(int dof) const
{ return m_weight[dof]; }
void ScaleWeight(int dof, MT_Scalar scale)
{ m_weight[dof] *= scale; }
// recursively update the global coordinates of this segment, 'global'
// is the global transformation from the parent segment
void UpdateTransform(const MT_Transform &global);
// get axis from rotation matrix for derivative computation
virtual MT_Vector3 Axis(int dof) const=0;
// update the angles using the dTheta's computed using the jacobian matrix
virtual bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*)=0;
virtual void Lock(int, IK_QJacobian&, MT_Vector3&) {}
virtual void UpdateAngleApply()=0;
// set joint limits
virtual void SetLimit(int, MT_Scalar, MT_Scalar) {};
// set joint weights (per axis)
virtual void SetWeight(int, MT_Scalar) {};
virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
// functions needed for pole vector constraint
void PrependBasis(const MT_Matrix3x3& mat);
void Reset();
// scale
virtual void Scale(float scale);
protected:
// num_DoF: number of degrees of freedom
IK_QSegment(int num_DoF, bool translational);
// remove child as a child of this segment
void RemoveChild(IK_QSegment *child);
// tree structure variables
IK_QSegment *m_parent;
IK_QSegment *m_child;
IK_QSegment *m_sibling;
IK_QSegment *m_composite;
// full transform =
// start * rest_basis * basis * translation
MT_Vector3 m_start;
MT_Matrix3x3 m_rest_basis;
MT_Matrix3x3 m_basis;
MT_Vector3 m_translation;
// original basis
MT_Matrix3x3 m_orig_basis;
MT_Vector3 m_orig_translation;
// maximum extension of this segment
MT_Scalar m_max_extension;
// accumulated transformations starting from root
MT_Point3 m_global_start;
MT_Transform m_global_transform;
// number degrees of freedom, (first) id of this segments DOF's
int m_num_DoF, m_DoF_id;
bool m_locked[3];
bool m_translational;
MT_Scalar m_weight[3];
};
class IK_QSphericalSegment : public IK_QSegment
{
public:
IK_QSphericalSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
bool ComputeClampRotation(MT_Vector3& clamp);
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
private:
MT_Matrix3x3 m_new_basis;
bool m_limit_x, m_limit_y, m_limit_z;
MT_Scalar m_min[2], m_max[2];
MT_Scalar m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
MT_Scalar m_locked_ax, m_locked_ay, m_locked_az;
};
class IK_QNullSegment : public IK_QSegment
{
public:
IK_QNullSegment();
bool UpdateAngle(const IK_QJacobian&, MT_Vector3&, bool*) { return false; }
void UpdateAngleApply() {}
MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
};
class IK_QRevoluteSegment : public IK_QSegment
{
public:
// axis: the axis of the DoF, in range 0..2
IK_QRevoluteSegment(int axis);
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
int m_axis;
MT_Scalar m_angle, m_new_angle;
bool m_limit;
MT_Scalar m_min, m_max;
};
class IK_QSwingSegment : public IK_QSegment
{
public:
// XZ DOF, uses one direct rotation
IK_QSwingSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
MT_Matrix3x3 m_new_basis;
bool m_limit_x, m_limit_z;
MT_Scalar m_min[2], m_max[2];
MT_Scalar m_max_x, m_max_z, m_offset_x, m_offset_z;
};
class IK_QElbowSegment : public IK_QSegment
{
public:
// XY or ZY DOF, uses two sequential rotations: first rotate around
// X or Z, then rotate around Y (twist)
IK_QElbowSegment(int axis);
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta);
void UpdateAngleApply();
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
int m_axis;
MT_Scalar m_twist, m_angle, m_new_twist, m_new_angle;
MT_Scalar m_cos_twist, m_sin_twist;
bool m_limit, m_limit_twist;
MT_Scalar m_min, m_max, m_min_twist, m_max_twist;
};
class IK_QTranslateSegment : public IK_QSegment
{
public:
// 1DOF, 2DOF or 3DOF translational segments
IK_QTranslateSegment(int axis1);
IK_QTranslateSegment(int axis1, int axis2);
IK_QTranslateSegment();
MT_Vector3 Axis(int dof) const;
bool UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp);
void Lock(int, IK_QJacobian&, MT_Vector3&);
void UpdateAngleApply();
void SetWeight(int axis, MT_Scalar weight);
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void Scale(float scale);
private:
int m_axis[3];
bool m_axis_enabled[3], m_limit[3];
MT_Vector3 m_new_translation;
MT_Scalar m_min[3], m_max[3];
};
#endif
|