Method UpdateLocalR

GTE/Graphics/IKController.cpp:345–419

}

bool IKController::Joint::UpdateLocalR(int32_t axis, std::vector<Goal> const& goals)
{
    Vector3<float> U = GetAxis(axis);
    float numer = 0.0f;
    float denom = 0.0f;

    float oldNorm = 0.0f;
    for (auto g : goalIndices)
    {
        auto const& goal = goals[g];
        Vector3<float> EmP = goal.GetEffectorPosition() - object->worldTransform.GetTranslation();
        Vector3<float> GmP = goal.GetTargetPosition() - object->worldTransform.GetTranslation();
        Vector3<float> GmE = goal.GetTargetPosition() - goal.GetEffectorPosition();
        oldNorm += Dot(GmE, GmE);
        Vector3<float> UxEmP = Cross(U, EmP);
        Vector3<float> UxUxEmP = Cross(U, UxEmP);
        numer += goal.weight * Dot(GmP, UxEmP);
        denom -= goal.weight * Dot(GmP, UxUxEmP);
    }

    if (numer * numer + denom * denom == 0.0f)
    {
        return false;
    }

    // Desired angle to rotate about axis(i).
    float theta = std::atan2(numer, denom);

    // Factor local rotation into Euler angles.
    EulerAngles<float> euler =
        Rotation<4, float>(object->localTransform.GetRotation())(0, 1, 2);

    // Clamp to range.
    float desired = euler.angle[axis] + theta;
    if (desired > minRotation[axis])
    {
        if (desired < maxRotation[axis])
        {
            euler.angle[axis] = desired;
        }
        else
        {
            theta = maxRotation[axis] - euler.angle[axis];
            euler.angle[axis] = maxRotation[axis];
        }
    }
    else
    {
        theta = minRotation[axis] - euler.angle[axis];
        euler.angle[axis] = minRotation[axis];
    }

    // Test whether step should be taken.
    float newNorm = 0.0f;
    Matrix3x3<float> rotate = Rotation<3, float>(AxisAngle<3, float>(U, theta));
    for (auto g : goalIndices)
    {
        auto const& goal = goals[g];