Fix too much memory usage for Cycles attribute map.
[blender.git] / intern / iksolver / intern / IK_QTask.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Original Author: Laurence
24  * Contributor(s): Brecht
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 /** \file iksolver/intern/IK_QTask.cpp
30  *  \ingroup iksolver
31  */
32
33
34 #include "IK_QTask.h"
35
36 // IK_QTask
37
38 IK_QTask::IK_QTask(
39     int size,
40     bool primary,
41     bool active,
42     const IK_QSegment *segment
43     ) :
44         m_size(size), m_primary(primary), m_active(active), m_segment(segment),
45         m_weight(1.0)
46 {
47 }
48
49 // IK_QPositionTask
50
51 IK_QPositionTask::IK_QPositionTask(
52     bool primary,
53     const IK_QSegment *segment,
54     const Vector3d& goal
55     ) :
56         IK_QTask(3, primary, true, segment), m_goal(goal)
57 {
58         // computing clamping length
59         int num;
60         const IK_QSegment *seg;
61
62         m_clamp_length = 0.0;
63         num = 0;
64
65         for (seg = m_segment; seg; seg = seg->Parent()) {
66                 m_clamp_length += seg->MaxExtension();
67                 num++;
68         }
69
70         m_clamp_length /= 2 * num;
71 }
72
73 void IK_QPositionTask::ComputeJacobian(IK_QJacobian& jacobian)
74 {
75         // compute beta
76         const Vector3d& pos = m_segment->GlobalEnd();
77
78         Vector3d d_pos = m_goal - pos;
79         double length = d_pos.norm();
80
81         if (length > m_clamp_length)
82                 d_pos = (m_clamp_length / length) * d_pos;
83         
84         jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
85
86         // compute derivatives
87         int i;
88         const IK_QSegment *seg;
89
90         for (seg = m_segment; seg; seg = seg->Parent()) {
91                 Vector3d p = seg->GlobalStart() - pos;
92
93                 for (i = 0; i < seg->NumberOfDoF(); i++) {
94                         Vector3d axis = seg->Axis(i) * m_weight;
95
96                         if (seg->Translational())
97                                 jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
98                         else {
99                                 Vector3d pa = p.cross(axis);
100                                 jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
101                         }
102                 }
103         }
104 }
105
106 double IK_QPositionTask::Distance() const
107 {
108         const Vector3d& pos = m_segment->GlobalEnd();
109         Vector3d d_pos = m_goal - pos;
110         return d_pos.norm();
111 }
112
113 // IK_QOrientationTask
114
115 IK_QOrientationTask::IK_QOrientationTask(
116     bool primary,
117     const IK_QSegment *segment,
118     const Matrix3d& goal
119     ) :
120         IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
121 {
122 }
123
124 void IK_QOrientationTask::ComputeJacobian(IK_QJacobian& jacobian)
125 {
126         // compute betas
127         const Matrix3d& rot = m_segment->GlobalTransform().linear();
128
129         Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
130
131         Vector3d d_rot;
132         d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
133                                 d_rotm(0, 2) - d_rotm(2, 0),
134                                 d_rotm(1, 0) - d_rotm(0, 1));
135
136         m_distance = d_rot.norm();
137
138         jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
139
140         // compute derivatives
141         int i;
142         const IK_QSegment *seg;
143
144         for (seg = m_segment; seg; seg = seg->Parent())
145                 for (i = 0; i < seg->NumberOfDoF(); i++) {
146
147                         if (seg->Translational())
148                                 jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
149                         else {
150                                 Vector3d axis = seg->Axis(i) * m_weight;
151                                 jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
152                         }
153                 }
154 }
155
156 // IK_QCenterOfMassTask
157 // Note: implementation not finished!
158
159 IK_QCenterOfMassTask::IK_QCenterOfMassTask(
160     bool primary,
161     const IK_QSegment *segment,
162     const Vector3d& goal_center
163     ) :
164         IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
165 {
166         m_total_mass_inv = ComputeTotalMass(m_segment);
167         if (!FuzzyZero(m_total_mass_inv))
168                 m_total_mass_inv = 1.0 / m_total_mass_inv;
169 }
170
171 double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
172 {
173         double mass = /*seg->Mass()*/ 1.0;
174
175         const IK_QSegment *seg;
176         for (seg = segment->Child(); seg; seg = seg->Sibling())
177                 mass += ComputeTotalMass(seg);
178         
179         return mass;
180 }
181
182 Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
183 {
184         Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
185
186         const IK_QSegment *seg;
187         for (seg = segment->Child(); seg; seg = seg->Sibling())
188                 center += ComputeCenter(seg);
189         
190         return center;
191 }
192
193 void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian& jacobian, Vector3d& center, const IK_QSegment *segment)
194 {
195         int i;
196         Vector3d p = center - segment->GlobalStart();
197
198         for (i = 0; i < segment->NumberOfDoF(); i++) {
199                 Vector3d axis = segment->Axis(i) * m_weight;
200                 axis *= /*segment->Mass()**/ m_total_mass_inv;
201                 
202                 if (segment->Translational())
203                         jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
204                 else {
205                         Vector3d pa = axis.cross(p);
206                         jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
207                 }
208         }
209         
210         const IK_QSegment *seg;
211         for (seg = segment->Child(); seg; seg = seg->Sibling())
212                 JacobianSegment(jacobian, center, seg);
213 }
214
215 void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian& jacobian)
216 {
217         Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
218
219         // compute beta
220         Vector3d d_pos = m_goal_center - center;
221
222         m_distance = d_pos.norm();
223
224 #if 0
225         if (m_distance > m_clamp_length)
226                 d_pos = (m_clamp_length / m_distance) * d_pos;
227 #endif
228         
229         jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
230
231         // compute derivatives
232         JacobianSegment(jacobian, center, m_segment);
233 }
234
235 double IK_QCenterOfMassTask::Distance() const
236 {
237         return m_distance;
238 }
239