2002-10-12 11:37:38 +00:00
|
|
|
#include "MT_Optimize.h"
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3& MT_Point3::operator+=(const MT_Vector3& v) {
|
|
|
|
|
m_co[0] += v[0]; m_co[1] += v[1]; m_co[2] += v[2];
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3& MT_Point3::operator-=(const MT_Vector3& v) {
|
|
|
|
|
m_co[0] -= v[0]; m_co[1] -= v[1]; m_co[2] -= v[2];
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3& MT_Point3::operator=(const MT_Vector3& v) {
|
|
|
|
|
m_co[0] = v[0]; m_co[1] = v[1]; m_co[2] = v[2];
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
|
Update SConscript.
Fix some warnings.
Merge with latest soc code.
What changed in IK lib:
Fully restructured, with components now as follows:
- IK_Solver: C <=> C++ interface
- IK_QSegment: base class for bone/segment with 0
to 3 DOF
- IK_QTask: base class for a task (currently there's
a position and a rotation task)
- IK_QJacobian: the Jacobian matrix, with SVD
decomposition, damping, etc
- IK_QJacobianSolver: the iterative solver
The exponential map parametrization is no longer used,
instead we have now:
- 3DOF and 2DOF XZ segments: directly update matrix
with Rodrigues' formula
- Other: Euler angles (no worries about singularities
here)
Computation of the Jacobian inverse has also changed:
- The SVD algorithm is now based on LAPACK code,
instead of NR, to avoid some problems with rounding
errors.
- When the problem is underconstrained (as is the case
most of the time), the SVD is computed for the transpose
of the Jacobian (faster).
- A new damping algorithm called the Selectively Damped
Least Squares is used, result in faster and more
stable convergence.
- Stiffness is implemented as if a weighted psuedo-inverse
was used.
Tree structure support.
Rotation limits:
- 3DOF and 2DOF XZ segments limits are based on a swing
(direct axis-angle over XZ) and twist/roll (rotation
over Y) decomposition. The swing region is an ellipse
on a sphere.
- Rotation limits are implemented using an inner clamping
loop: as long as there is a violation, a violating DOF
is clamped and removed from the Jacobian, and the solution
is recomputed.
Convergence checking is based now on the max norm of angle
change, or the maximum number of iterations.
2005-08-27 13:45:19 +00:00
|
|
|
GEN_INLINE MT_Point3& MT_Point3::operator=(const MT_Point3& v) {
|
|
|
|
|
m_co[0] = v[0]; m_co[1] = v[1]; m_co[2] = v[2];
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
|
2002-10-12 11:37:38 +00:00
|
|
|
GEN_INLINE MT_Scalar MT_Point3::distance(const MT_Point3& p) const {
|
|
|
|
|
return (p - *this).length();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Scalar MT_Point3::distance2(const MT_Point3& p) const {
|
|
|
|
|
return (p - *this).length2();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3 MT_Point3::lerp(const MT_Point3& p, MT_Scalar t) const {
|
|
|
|
|
return MT_Point3(m_co[0] + (p[0] - m_co[0]) * t,
|
|
|
|
|
m_co[1] + (p[1] - m_co[1]) * t,
|
|
|
|
|
m_co[2] + (p[2] - m_co[2]) * t);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3 operator+(const MT_Point3& p, const MT_Vector3& v) {
|
|
|
|
|
return MT_Point3(p[0] + v[0], p[1] + v[1], p[2] + v[2]);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3 operator-(const MT_Point3& p, const MT_Vector3& v) {
|
|
|
|
|
return MT_Point3(p[0] - v[0], p[1] - v[1], p[2] - v[2]);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Vector3 operator-(const MT_Point3& p1, const MT_Point3& p2) {
|
|
|
|
|
return MT_Vector3(p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2]);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Scalar MT_distance(const MT_Point3& p1, const MT_Point3& p2) {
|
|
|
|
|
return p1.distance(p2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Scalar MT_distance2(const MT_Point3& p1, const MT_Point3& p2) {
|
|
|
|
|
return p1.distance2(p2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
GEN_INLINE MT_Point3 MT_lerp(const MT_Point3& p1, const MT_Point3& p2, MT_Scalar t) {
|
|
|
|
|
return p1.lerp(p2, t);
|
|
|
|
|
}
|
