diff options
Diffstat (limited to 'intern/iksolver/intern/IK_QJacobian.h')
-rw-r--r-- | intern/iksolver/intern/IK_QJacobian.h | 62 |
1 files changed, 24 insertions, 38 deletions
diff --git a/intern/iksolver/intern/IK_QJacobian.h b/intern/iksolver/intern/IK_QJacobian.h index b4b5a0402e6..f541866c6a7 100644 --- a/intern/iksolver/intern/IK_QJacobian.h +++ b/intern/iksolver/intern/IK_QJacobian.h @@ -31,44 +31,36 @@ * \ingroup iksolver */ +#pragma once -#ifndef __IK_QJACOBIAN_H__ - -#define __IK_QJACOBIAN_H__ - -#include "TNT/cmat.h" -#include <vector> -#include "MT_Vector3.h" +#include "IK_Math.h" class IK_QJacobian { public: - typedef TNT::Matrix<MT_Scalar> TMatrix; - typedef TNT::Vector<MT_Scalar> TVector; - IK_QJacobian(); ~IK_QJacobian(); // Call once to initialize void ArmMatrices(int dof, int task_size); - void SetDoFWeight(int dof, MT_Scalar weight); + void SetDoFWeight(int dof, double weight); // Iteratively called - void SetBetas(int id, int size, const MT_Vector3& v); - void SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight); + void SetBetas(int id, int size, const Vector3d& v); + void SetDerivatives(int id, int dof_id, const Vector3d& v, double norm_weight); void Invert(); - MT_Scalar AngleUpdate(int dof_id) const; - MT_Scalar AngleUpdateNorm() const; + double AngleUpdate(int dof_id) const; + double AngleUpdateNorm() const; // DoF locking for inner clamping loop - void Lock(int dof_id, MT_Scalar delta); + void Lock(int dof_id, double delta); // Secondary task bool ComputeNullProjection(); - void Restrict(TVector& d_theta, TMatrix& null); + void Restrict(VectorXd& d_theta, MatrixXd& nullspace); void SubTask(IK_QJacobian& jacobian); private: @@ -80,41 +72,35 @@ private: bool m_transpose; // the jacobian matrix and it's null space projector - TMatrix m_jacobian, m_jacobian_tmp; - TMatrix m_null; + MatrixXd m_jacobian, m_jacobian_tmp; + MatrixXd m_nullspace; /// the vector of intermediate betas - TVector m_beta; + VectorXd m_beta; /// the vector of computed angle changes - TVector m_d_theta; - TVector m_d_norm_weight; + VectorXd m_d_theta; + VectorXd m_d_norm_weight; /// space required for SVD computation + VectorXd m_svd_w; + MatrixXd m_svd_v; + MatrixXd m_svd_u; - TVector m_svd_w; - TMatrix m_svd_v; - TMatrix m_svd_u; - TVector m_work1; - TVector m_work2; - - TMatrix m_svd_u_t; - TVector m_svd_u_beta; + VectorXd m_svd_u_beta; // space required for SDLS bool m_sdls; - TVector m_norm; - TVector m_d_theta_tmp; - MT_Scalar m_min_damp; + VectorXd m_norm; + VectorXd m_d_theta_tmp; + double m_min_damp; // null space task vector - TVector m_alpha; + VectorXd m_alpha; // dof weighting - TVector m_weight; - TVector m_weight_sqrt; + VectorXd m_weight; + VectorXd m_weight_sqrt; }; -#endif - |