Blender  V3.3
IK_QTask.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: GPL-2.0-or-later
2  * Copyright 2001-2002 NaN Holding BV. All rights reserved. */
3 
8 #include "IK_QTask.h"
9 
10 // IK_QTask
11 
12 IK_QTask::IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment)
13  : m_size(size), m_primary(primary), m_active(active), m_segment(segment), m_weight(1.0)
14 {
15 }
16 
17 // IK_QPositionTask
18 
19 IK_QPositionTask::IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal)
20  : IK_QTask(3, primary, true, segment), m_goal(goal)
21 {
22  // computing clamping length
23  int num;
24  const IK_QSegment *seg;
25 
26  m_clamp_length = 0.0;
27  num = 0;
28 
29  for (seg = m_segment; seg; seg = seg->Parent()) {
30  m_clamp_length += seg->MaxExtension();
31  num++;
32  }
33 
34  m_clamp_length /= 2 * num;
35 }
36 
38 {
39  // compute beta
40  const Vector3d &pos = m_segment->GlobalEnd();
41 
42  Vector3d d_pos = m_goal - pos;
43  double length = d_pos.norm();
44 
45  if (length > m_clamp_length)
46  d_pos = (m_clamp_length / length) * d_pos;
47 
48  jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
49 
50  // compute derivatives
51  int i;
52  const IK_QSegment *seg;
53 
54  for (seg = m_segment; seg; seg = seg->Parent()) {
55  Vector3d p = seg->GlobalStart() - pos;
56 
57  for (i = 0; i < seg->NumberOfDoF(); i++) {
58  Vector3d axis = seg->Axis(i) * m_weight;
59 
60  if (seg->Translational())
61  jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
62  else {
63  Vector3d pa = p.cross(axis);
64  jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
65  }
66  }
67  }
68 }
69 
71 {
72  const Vector3d &pos = m_segment->GlobalEnd();
73  Vector3d d_pos = m_goal - pos;
74  return d_pos.norm();
75 }
76 
77 // IK_QOrientationTask
78 
80  const IK_QSegment *segment,
81  const Matrix3d &goal)
82  : IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
83 {
84 }
85 
87 {
88  // compute betas
89  const Matrix3d &rot = m_segment->GlobalTransform().linear();
90 
91  Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
92 
93  Vector3d d_rot;
94  d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
95  d_rotm(0, 2) - d_rotm(2, 0),
96  d_rotm(1, 0) - d_rotm(0, 1));
97 
98  m_distance = d_rot.norm();
99 
100  jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
101 
102  // compute derivatives
103  int i;
104  const IK_QSegment *seg;
105 
106  for (seg = m_segment; seg; seg = seg->Parent())
107  for (i = 0; i < seg->NumberOfDoF(); i++) {
108 
109  if (seg->Translational())
110  jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
111  else {
112  Vector3d axis = seg->Axis(i) * m_weight;
113  jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
114  }
115  }
116 }
117 
118 // IK_QCenterOfMassTask
119 // NOTE: implementation not finished!
120 
122  const IK_QSegment *segment,
123  const Vector3d &goal_center)
124  : IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
125 {
126  m_total_mass_inv = ComputeTotalMass(m_segment);
127  if (!FuzzyZero(m_total_mass_inv))
128  m_total_mass_inv = 1.0 / m_total_mass_inv;
129 }
130 
131 double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
132 {
133  double mass = /*seg->Mass()*/ 1.0;
134 
135  const IK_QSegment *seg;
136  for (seg = segment->Child(); seg; seg = seg->Sibling())
137  mass += ComputeTotalMass(seg);
138 
139  return mass;
140 }
141 
142 Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
143 {
144  Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
145 
146  const IK_QSegment *seg;
147  for (seg = segment->Child(); seg; seg = seg->Sibling())
148  center += ComputeCenter(seg);
149 
150  return center;
151 }
152 
153 void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
154  Vector3d &center,
155  const IK_QSegment *segment)
156 {
157  int i;
158  Vector3d p = center - segment->GlobalStart();
159 
160  for (i = 0; i < segment->NumberOfDoF(); i++) {
161  Vector3d axis = segment->Axis(i) * m_weight;
162  axis *= /*segment->Mass()**/ m_total_mass_inv;
163 
164  if (segment->Translational())
165  jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
166  else {
167  Vector3d pa = axis.cross(p);
168  jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
169  }
170  }
171 
172  const IK_QSegment *seg;
173  for (seg = segment->Child(); seg; seg = seg->Sibling())
174  JacobianSegment(jacobian, center, seg);
175 }
176 
178 {
179  Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
180 
181  // compute beta
182  Vector3d d_pos = m_goal_center - center;
183 
184  m_distance = d_pos.norm();
185 
186 #if 0
187  if (m_distance > m_clamp_length)
188  d_pos = (m_clamp_length / m_distance) * d_pos;
189 #endif
190 
191  jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
192 
193  // compute derivatives
194  JacobianSegment(jacobian, center, m_segment);
195 }
196 
198 {
199  return m_distance;
200 }
NSNotificationCenter * center
static bool FuzzyZero(double x)
Definition: IK_Math.h:23
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void ComputeJacobian(IK_QJacobian &jacobian)
Definition: IK_QTask.cpp:177
IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d &center)
Definition: IK_QTask.cpp:121
double Distance() const
Definition: IK_QTask.cpp:197
void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
void SetBetas(int id, int size, const Vector3d &v)
IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal)
Definition: IK_QTask.cpp:79
void ComputeJacobian(IK_QJacobian &jacobian)
Definition: IK_QTask.cpp:86
void ComputeJacobian(IK_QJacobian &jacobian)
Definition: IK_QTask.cpp:37
double Distance() const
Definition: IK_QTask.cpp:70
IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal)
Definition: IK_QTask.cpp:19
int NumberOfDoF() const
Definition: IK_QSegment.h:77
int DoFId() const
Definition: IK_QSegment.h:83
const Vector3d GlobalStart() const
Definition: IK_QSegment.h:104
const Affine3d & GlobalTransform() const
Definition: IK_QSegment.h:115
const Vector3d GlobalEnd() const
Definition: IK_QSegment.h:109
IK_QSegment * Sibling() const
Definition: IK_QSegment.h:58
bool Translational() const
Definition: IK_QSegment.h:121
const double MaxExtension() const
Definition: IK_QSegment.h:94
virtual Vector3d Axis(int dof) const =0
IK_QSegment * Parent() const
Definition: IK_QSegment.h:63
const IK_QSegment * m_segment
Definition: IK_QTask.h:74
double m_weight
Definition: IK_QTask.h:75
int m_size
Definition: IK_QTask.h:71
int m_id
Definition: IK_QTask.h:70
IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment)
Definition: IK_QTask.cpp:12
#define rot(x, k)
uint pos
Segment< FEdge *, Vec3r > segment
bool active
all scheduled work for the GPU.
T length(const vec_base< T, Size > &a)