C++ Quaternion::AddScaledVector方法代码示例

void Contact::ApplyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], marb penetration) {
    const marb angularLimit = (marb)0.2f;
    marb angularMove[2];
    marb linearMove[2];

    marb totalInertia = 0;
    marb linearInertia[2];
    marb angularInertia[2];

    // We need to work out the inertia of each object in the direction
    // of the contact normal, due to angular inertia only.
    for (unsigned i = 0; i < 2; i++) if (body[i]) {
        Matrix3 inverseInertiaTensor;
        body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);

        // Use the same procedure as for calculating frictionless
        // velocity change to work out the angular inertia.
        Vector3 angularInertiaWorld =
            relativeContactPosition[i] % contactNormal;
        angularInertiaWorld =
            inverseInertiaTensor.Transform(angularInertiaWorld);
        angularInertiaWorld =
            angularInertiaWorld % relativeContactPosition[i];
        angularInertia[i] =
            angularInertiaWorld * contactNormal;

        // The linear component is simply the inverse mass
        linearInertia[i] = body[i]->GetInverseMass();

        // Keep track of the total inertia from all components
        totalInertia += linearInertia[i] + angularInertia[i];

        // We break the loop here so that the totalInertia value is
        // completely calculated (by both iterations) before
        // continuing.
    }

    // Loop through again calculating and applying the changes
    for (unsigned i = 0; i < 2; i++) if (body[i]) {
        // The linear and angular movements required are in proportion to
        // the two inverse inertias.
        marb sign = (i == 0)?1:-1;
        angularMove[i] =
            sign * penetration * (angularInertia[i] / totalInertia);
        linearMove[i] =
            sign * penetration * (linearInertia[i] / totalInertia);

        // To avoid angular projections that are too great (when mass is large
        // but inertia tensor is small) limit the angular move.
        Vector3 projection = relativeContactPosition[i];
        projection.AddScaledVector(
            contactNormal,
            -relativeContactPosition[i].ScalarProduct(contactNormal)
            );

        // Use the small angle approximation for the sine of the angle (i.e.
        // the magnitude would be sine(angularLimit) * projection.magnitude
        // but we approximate sine(angularLimit) to angularLimit).
        marb maxMagnitude = angularLimit * projection.Magnitude();

        if (angularMove[i] < -maxMagnitude) {
            marb totalMove = angularMove[i] + linearMove[i];
            angularMove[i] = -maxMagnitude;
            linearMove[i] = totalMove - angularMove[i];

        } else if (angularMove[i] > maxMagnitude) {
            marb totalMove = angularMove[i] + linearMove[i];
            angularMove[i] = maxMagnitude;
            linearMove[i] = totalMove - angularMove[i];
        }

        // We have the linear amount of movement required by turning
        // the rigid body (in angularMove[i]). We now need to
        // calculate the desired rotation to achieve that.
        if (angularMove[i] == 0) {
            // Easy case - no angular movement means no rotation.
            angularChange[i].Clear();

        } else {
            // Work out the direction we'd like to rotate in.
            Vector3 targetAngularDirection =
                relativeContactPosition[i].VectorProduct(contactNormal);

            Matrix3 inverseInertiaTensor;
            body[i]->GetInverseInertiaTensorWorld(&inverseInertiaTensor);

            // Work out the direction we'd need to rotate to achieve that
            angularChange[i] =
                inverseInertiaTensor.Transform(targetAngularDirection) *
                (angularMove[i] / angularInertia[i]);
        }

        // Velocity change is easier - it is just the linear movement
        // along the contact normal.
        linearChange[i] = contactNormal * linearMove[i];

        // Now we can start to apply the values we've calculated.
        // Apply the linear movement
        Vector3 pos;
        body[i]->GetPosition(&pos);
//.........这里部分代码省略.........

void RigidBodyContact::ApplyPositionChange(Vector3* linearChange, Vector3* angularChange, float penetration)
{
    float angularLimit = 5.0f;
    float angularMove[2];
    float linearMove[2];
    float linearInertia[2];
    float angularInteria[2];
    float totalInertia = 0.0f;

    // Calculate the angular inertia using the same process as for calculating frictionless velocity change
    for (int i = 0; i < 2; i++)
    {
        if (Body[i] != NULL)
        {
            Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
            Vector3 angularInertiaWorld = m_relativeContactPosition[i].Cross(ContactNormal);
            angularInertiaWorld = angularInertiaWorld*inverseInertiaTensor; // TODO physics: is order correct here?
            angularInertiaWorld = angularInertiaWorld.Cross(m_relativeContactPosition[i]);
            angularInteria[i] = angularInertiaWorld.Dot(ContactNormal);

            // The linear inertia component is just the inverse mass
            linearInertia[i] = Body[i]->GetInverseMass();

            // Combine angular and linear to calculate total
            totalInertia += linearInertia[i] + angularInteria[i];
        }
    }

    // Calculate and apply changes
    for (int i = 0; i < 2; i++)
    {
        if (Body[i] != NULL)
        {
            // The linear and angular movements required are in proportion to the two inverse inertias
            float sign = (i == 0) ? 1.f : -1.f;
            angularMove[i] = sign * penetration * (angularInteria[i] / totalInertia);
            linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia);

            // To avoid angular projections that are too great (when mass is large but inertia
            // tensor is small), limit the angular move
            Vector3 projection = m_relativeContactPosition[i];
            projection += ContactNormal * -m_relativeContactPosition[i].Dot(ContactNormal);

            // Use the small angle approximation for the sine of the angle (i.e. the magnitude would be
            // sin(angularLimit) * projection.magnitude, but we approximate sin(angularLimit) to angularLimit.
            float maxMagnitude = angularLimit * projection.Magnitude();

            if (angularMove[i] < -maxMagnitude)
            {
                float totalMove = angularMove[i] + linearMove[i];
                angularMove[i] = -maxMagnitude;
                linearMove[i] = totalMove - angularMove[i];
            }
            else if (angularMove[i] > maxMagnitude)
            {
                float totalMove = angularMove[i] + linearMove[i];
                angularMove[i] = maxMagnitude;
                linearMove[i] = totalMove - angularMove[i];
            }

            // We have the linear amount of movement required by turning the rigid body (angularMove[i]).
            // We now need to calculate the desired rotation to achieve that.
            if (Approximately(angularMove[i], 0.f))
            {
                // Easy case - no angular movement means no rotation.
                angularChange[i] = Vector3::Zero;
            }
            else
            {
                // Work out the direction we'd like to rotate in
                Vector3 targetAngularDirection = m_relativeContactPosition[i].Cross(ContactNormal);     // TODO physics: should this get normalized?

                // Work out the direction we'd need to rotate to achieve that
                Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
                angularChange[i] = (targetAngularDirection*inverseInertiaTensor)* (angularMove[i] / angularInteria[i]);      // TODO physics: is order correct here?
            }

            // Velocity change is just the linear movement along the contact normal
            linearChange[i] = ContactNormal * linearMove[i];

            // Apply the linear movement
            Vector3 pos = Body[i]->GetPosition();
            pos += linearMove[i] * ContactNormal;
            Body[i]->SetPosition(pos);

            // Apply the change in orientation
            Quaternion rot = Body[i]->GetRotation();
            rot.AddScaledVector(angularChange[i], 1.0);
            Body[i]->SetRotation(rot);

            // TODO calculate derived data for bodies that are not awake
        }
    }
}