bartvdbraak/blender
1
0
Fork 4
Code Issues 4 Pull Requests 1 Actions 1 Packages Projects Releases Wiki Activity

Bugfix; rotation limits for < 3 DOF bones were using wrong reference

Browse Source 
rotation, causing incorrect limits if there was already a pose transform.
This commit is contained in:
Brecht Van Lommel 2005-08-29 12:06:23 +00:00
parent 30be6f0e67
commit c921ee25cc
2 changed files with 57 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
intern/iksolver/intern/IK_QSegment.cpp
View File

@ -55,6 +55,22 @@ static MT_Matrix3x3 RotationMatrix(MT_Scalar angle, int axis)
return RotationMatrix(sin(angle), cos(angle), axis);
}
static MT_Scalar EulerAngleFromMatrix(const MT_Matrix3x3& R, int axis)
{
MT_Scalar t = sqrt(R[0][0]*R[0][0] + R[0][1]*R[0][1]);
if (t > 16.0*MT_EPSILON) {
if (axis == 0) return atan2(R[1][2], R[2][2]);
else if(axis == 1) return atan2(-R[0][2], t);
else return atan2(R[0][1], R[0][0]);
} else {
if (axis == 0) return atan2(-R[2][1], R[1][1]);
else if(axis == 1) return atan2(-R[0][2], t);
else return 0.0f;
}
}
static MT_Scalar safe_acos(MT_Scalar f)
{
if (f <= -1.0f)
@ -173,10 +189,11 @@ void IK_QSegment::SetTransform(
m_start = start;
m_rest_basis = rest_basis;
m_basis = basis;
m_translation = MT_Vector3(0, length, 0);
m_orig_basis = m_basis;
m_orig_basis = basis;
SetBasis(basis);
m_translation = MT_Vector3(0, length, 0);
m_orig_translation = m_translation;
}
@ -485,6 +502,18 @@ IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
{
}
void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis)
{
if (m_axis == 1) {
m_angle = ComputeTwist(basis);
m_basis = ComputeTwistMatrix(m_angle);
}
else {
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_basis = RotationMatrix(m_angle, m_axis);
}
}
MT_Vector3 IK_QRevoluteSegment::Axis(int) const
{
return m_global_transform.getBasis().getColumn(m_axis);
@ -524,8 +553,7 @@ void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
void IK_QRevoluteSegment::UpdateAngleApply()
{
m_angle = m_new_angle;
m_basis = m_orig_basis*RotationMatrix(m_angle, m_axis);
m_basis = RotationMatrix(m_angle, m_axis);
}
void IK_QRevoluteSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)
@ -556,6 +584,12 @@ IK_QSwingSegment::IK_QSwingSegment()
{
}
void IK_QSwingSegment::SetBasis(const MT_Matrix3x3& basis)
{
m_basis = basis;
RemoveTwist(m_basis);
}
MT_Vector3 IK_QSwingSegment::Axis(int dof) const
{
return m_global_transform.getBasis().getColumn((dof==0)? 0: 2);
@ -736,6 +770,17 @@ IK_QElbowSegment::IK_QElbowSegment(int axis)
{
}
void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis)
{
m_basis = basis;
m_twist = ComputeTwist(m_basis);
RemoveTwist(m_basis);
m_angle = EulerAngleFromMatrix(basis, m_axis);
m_basis = RotationMatrix(m_angle, m_axis)*ComputeTwistMatrix(m_twist);
}
MT_Vector3 IK_QElbowSegment::Axis(int dof) const
{
if (dof == 0) {
@ -818,7 +863,7 @@ void IK_QElbowSegment::UpdateAngleApply()
MT_Matrix3x3 A = RotationMatrix(m_angle, m_axis);
MT_Matrix3x3 T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
m_basis = m_orig_basis*A*T;
m_basis = A*T;
}
void IK_QElbowSegment::SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax)

6
intern/iksolver/intern/IK_QSegment.h
View File

@ -157,6 +157,8 @@ public:
// set joint weights (per axis)
virtual void SetWeight(int, MT_Scalar) {};
virtual void SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; }
protected:
// num_DoF: number of degrees of freedom
@ -228,6 +230,7 @@ public:
void UpdateAngleApply() {}
MT_Vector3 Axis(int) const { return MT_Vector3(0, 0, 0); }
void SetBasis(const MT_Matrix3x3&) { m_basis.setIdentity(); }
};
class IK_QRevoluteSegment : public IK_QSegment
@ -244,6 +247,7 @@ public:
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
int m_axis;
@ -266,6 +270,7 @@ public:
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
MT_Matrix3x3 m_new_basis;
@ -288,6 +293,7 @@ public:
void SetLimit(int axis, MT_Scalar lmin, MT_Scalar lmax);
void SetWeight(int axis, MT_Scalar weight);
void SetBasis(const MT_Matrix3x3& basis);
private:
int m_axis;