b2d990d548985b515c825a7c41a7b30246848a76
[blender-staging.git] / intern / iksolver / intern / IK_QJacobian.h
1
2 /**
3  * $Id$
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version. The Blender
10  * Foundation also sells licenses for use in proprietary software under
11  * the Blender License.  See http://www.blender.org/BL/ for information
12  * about this.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22  *
23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24  * All rights reserved.
25  *
26  * The Original Code is: all of this file.
27  *
28  * Original Author: Laurence
29  * Contributor(s): Brecht
30  *
31  * ***** END GPL/BL DUAL LICENSE BLOCK *****
32  */
33
34 #ifndef NAN_INCLUDED_IK_QJacobian_h
35
36 #define NAN_INCLUDED_IK_QJacobian_h
37
38 #include "TNT/cmat.h"
39 #include "TNT/fmat.h"
40 #include <vector>
41 #include "MT_Vector3.h"
42
43 class IK_QJacobian
44 {
45 public:
46         typedef TNT::Matrix<MT_Scalar> TMatrix;
47         typedef TNT::Vector<MT_Scalar>TVector;
48
49         IK_QJacobian();
50         ~IK_QJacobian();
51
52         // Call once to initialize
53         void ArmMatrices(int dof, int task_size, int tasks);
54         void SetDoFWeight(int dof, MT_Scalar weight);
55
56         // Iteratively called
57         void SetBetas(int id, int size, const MT_Vector3& v);
58         void SetDerivatives(int id, int dof_id, const MT_Vector3& v);
59
60         void Invert();
61
62         MT_Scalar AngleUpdate(int dof_id) const;
63         MT_Scalar AngleUpdateNorm() const;
64
65         // DoF locking for inner clamping loop
66         void Lock(int dof_id, MT_Scalar delta);
67
68         // Secondary task
69         bool ComputeNullProjection();
70
71         void Restrict(TVector& d_theta, TMatrix& null);
72         void SubTask(IK_QJacobian& jacobian);
73
74 #if 0
75         void SetSecondary(int dof_id, MT_Scalar d);
76         void SolveSecondary();
77 #endif
78
79 private:
80         
81         void InvertSDLS();
82         void InvertDLS();
83
84         int m_dof, m_task_size, m_tasks;
85         bool m_transpose;
86
87         // the jacobian matrix and it's null space projector
88         TMatrix m_jacobian;
89         TMatrix m_null;
90
91         /// the vector of intermediate betas
92         TVector m_beta;
93
94         /// the vector of computed angle changes
95         TVector m_d_theta;
96
97         /// space required for SVD computation
98
99         TVector m_svd_w;
100     TVector m_svd_work_space;        
101         TMatrix m_svd_v;
102         TMatrix m_svd_u;
103
104         TMatrix m_svd_u_t;
105         TVector m_svd_u_beta;
106
107         // space required for SDLS
108
109         bool m_sdls;
110         TVector m_norm;
111         TVector m_d_theta_tmp;
112         MT_Scalar m_min_damp;
113
114         // null space task vector
115         TVector m_alpha;
116
117         // dof weighting
118         TVector m_weight;
119         TVector m_weight_sqrt;
120 };
121
122 #endif
123