Blender  V3.3
IK_QJacobian.h
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 #pragma once
9 
10 #include "IK_Math.h"
11 
12 class IK_QJacobian {
13  public:
14  IK_QJacobian();
15  ~IK_QJacobian();
16 
17  // Call once to initialize
18  void ArmMatrices(int dof, int task_size);
19  void SetDoFWeight(int dof, double weight);
20 
21  // Iteratively called
22  void SetBetas(int id, int size, const Vector3d &v);
23  void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight);
24 
25  void Invert();
26 
27  double AngleUpdate(int dof_id) const;
28  double AngleUpdateNorm() const;
29 
30  // DoF locking for inner clamping loop
31  void Lock(int dof_id, double delta);
32 
33  // Secondary task
34  bool ComputeNullProjection();
35 
36  void Restrict(VectorXd &d_theta, MatrixXd &nullspace);
37  void SubTask(IK_QJacobian &jacobian);
38 
39  private:
40  void InvertSDLS();
41  void InvertDLS();
42 
43  int m_dof, m_task_size;
44  bool m_transpose;
45 
46  // the jacobian matrix and its null space projector
47  MatrixXd m_jacobian, m_jacobian_tmp;
48  MatrixXd m_nullspace;
49 
51  VectorXd m_beta;
52 
54  VectorXd m_d_theta;
55  VectorXd m_d_norm_weight;
56 
58  VectorXd m_svd_w;
59  MatrixXd m_svd_v;
60  MatrixXd m_svd_u;
61 
62  VectorXd m_svd_u_beta;
63 
64  // space required for SDLS
65 
66  bool m_sdls;
67  VectorXd m_norm;
68  VectorXd m_d_theta_tmp;
69  double m_min_damp;
70 
71  // null space task vector
72  VectorXd m_alpha;
73 
74  // dof weighting
75  VectorXd m_weight;
76  VectorXd m_weight_sqrt;
77 };
ATTR_WARN_UNUSED_RESULT const BMVert * v
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
void Lock(int dof_id, double delta)
void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
void SetDoFWeight(int dof, double weight)
void Restrict(VectorXd &d_theta, MatrixXd &nullspace)
void SubTask(IK_QJacobian &jacobian)
double AngleUpdateNorm() const
void SetBetas(int id, int size, const Vector3d &v)
double AngleUpdate(int dof_id) const
void ArmMatrices(int dof, int task_size)
bool ComputeNullProjection()