Update SConscript.
[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 <vector>
40 #include "MT_Vector3.h"
41
42 class IK_QJacobian
43 {
44 public:
45         typedef TNT::Matrix<MT_Scalar> TMatrix;
46         typedef TNT::Vector<MT_Scalar> TVector;
47
48         IK_QJacobian();
49         ~IK_QJacobian();
50
51         // Call once to initialize
52         void ArmMatrices(int dof, int task_size, int tasks);
53         void SetDoFWeight(int dof, MT_Scalar weight);
54
55         // Iteratively called
56         void SetBetas(int id, int size, const MT_Vector3& v);
57         void SetDerivatives(int id, int dof_id, const MT_Vector3& v);
58
59         void Invert();
60
61         MT_Scalar AngleUpdate(int dof_id) const;
62         MT_Scalar AngleUpdateNorm() const;
63
64         // DoF locking for inner clamping loop
65         void Lock(int dof_id, MT_Scalar delta);
66
67         // Secondary task
68         bool ComputeNullProjection();
69
70         void Restrict(TVector& d_theta, TMatrix& null);
71         void SubTask(IK_QJacobian& jacobian);
72
73 private:
74         
75         void InvertSDLS();
76         void InvertDLS();
77
78         int m_dof, m_task_size, m_tasks;
79         bool m_transpose;
80
81         // the jacobian matrix and it's null space projector
82         TMatrix m_jacobian, m_jacobian_t;
83         TMatrix m_null;
84
85         /// the vector of intermediate betas
86         TVector m_beta;
87
88         /// the vector of computed angle changes
89         TVector m_d_theta;
90
91         /// space required for SVD computation
92
93         TVector m_svd_w;
94         TMatrix m_svd_v;
95         TMatrix m_svd_u;
96     TVector m_work1;
97     TVector m_work2;
98
99         TMatrix m_svd_u_t;
100         TVector m_svd_u_beta;
101
102         // space required for SDLS
103
104         bool m_sdls;
105         TVector m_norm;
106         TVector m_d_theta_tmp;
107         MT_Scalar m_min_damp;
108
109         // null space task vector
110         TVector m_alpha;
111
112         // dof weighting
113         TVector m_weight;
114         TVector m_weight_sqrt;
115 };
116
117 #endif
118