path: root/indra/newview/llbreastmotion.cpp

/**
 * @file llbreastmotion.cpp
 * @brief Implementation of LLBreastMotion class.
 *
 * $LicenseInfo:firstyear=2011&license=viewerlgpl$
 * Second Life Viewer Source Code
 * Copyright (C) 2011, Linden Research, Inc.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation;
 * version 2.1 of the License only.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 * $/LicenseInfo$
 */

//-----------------------------------------------------------------------------
// Header Files
//-----------------------------------------------------------------------------
#include "llviewerprecompiledheaders.h"
#include "linden_common.h"

#include "m3math.h"
#include "v3dmath.h"

#include "llbreastmotion.h"
#include "llcharacter.h"
#include "llviewercontrol.h"
#include "llviewervisualparam.h"
#include "llvoavatarself.h"

#define MIN_REQUIRED_PIXEL_AREA_BREAST_MOTION 0.f;

#define N_PARAMS 2

// User-set params
static const std::string breast_param_names_user[N_PARAMS] =
{
    "Breast_Female_Cleavage_Driver",
    "Breast_Gravity_Driver"
};

// Params driven by this algorithm
static const std::string breast_param_names_driven[N_PARAMS] =
{
    "Breast_Female_Cleavage",
    "Breast_Gravity"
};



LLBreastMotion::LLBreastMotion(const LLUUID &id) :
    LLMotion(id),
    mCharacter(NULL)
{
    mName = "breast_motion";
    mChestState = new LLJointState;

    mBreastMassParam = (F32)1.0;
    mBreastDragParam = LLVector3((F32)0.1, (F32)0.1, (F32)0.1);
    mBreastSmoothingParam = (U32)2;
    mBreastGravityParam = (F32)0.0;

    mBreastSpringParam = LLVector3((F32)3.0, (F32)0.0, (F32)3.0);
    mBreastGainParam = LLVector3((F32)50.0, (F32)0.0, (F32)50.0);
    mBreastDampingParam = LLVector3((F32)0.3, (F32)0.0, (F32)0.3);
    mBreastMaxVelocityParam = LLVector3((F32)10.0, (F32)0.0, (F32)10.0);

    mBreastParamsUser[0] = mBreastParamsUser[1] = mBreastParamsUser[2] = NULL;
    mBreastParamsDriven[0] = mBreastParamsDriven[1] = mBreastParamsDriven[2] = NULL;

    mCharLastPosition_world_pt = LLVector3(0,0,0);
    mCharLastVelocity_local_vec = LLVector3(0,0,0);
    mCharLastAcceleration_local_vec = LLVector3(0,0,0);
    mBreastLastPosition_local_pt = LLVector3(0,0,0);
    mBreastLastUpdatePosition_local_pt = LLVector3(0,0,0);
    mBreastVelocity_local_vec = LLVector3(0,0,0);
}

LLBreastMotion::~LLBreastMotion()
{
}

BOOL LLBreastMotion::onActivate()
{
    return TRUE;
}

void LLBreastMotion::onDeactivate()
{
}

LLMotion::LLMotionInitStatus LLBreastMotion::onInitialize(LLCharacter *character)
{
    mCharacter = character;

    if (!mChestState->setJoint(character->getJoint("mChest")))
    {
        return STATUS_FAILURE;
    }

    mChestState->setUsage(LLJointState::ROT);
    addJointState( mChestState );

    for (U32 i=0; i < N_PARAMS; i++)
    {
        mBreastParamsUser[i] = NULL;
        mBreastParamsDriven[i] = NULL;
        mBreastParamsMin[i] = 0;
        mBreastParamsMax[i] = 0;
        if (breast_param_names_user[i] != "" && breast_param_names_driven[i] != "")
        {
            mBreastParamsUser[i] = (LLViewerVisualParam*)mCharacter->getVisualParam(breast_param_names_user[i].c_str());
            mBreastParamsDriven[i] = (LLViewerVisualParam*)mCharacter->getVisualParam(breast_param_names_driven[i].c_str());
            if (mBreastParamsDriven[i])
            {
                mBreastParamsMin[i] = mBreastParamsDriven[i]->getMinWeight();
                mBreastParamsMax[i] = mBreastParamsDriven[i]->getMaxWeight();
            }
        }
    }

    mTimer.reset();
    return STATUS_SUCCESS;
}

F32 LLBreastMotion::getMinPixelArea()
{
    return MIN_REQUIRED_PIXEL_AREA_BREAST_MOTION;
}


F32 LLBreastMotion::calculateTimeDelta()
{
    const F32 time = mTimer.getElapsedTimeF32();
    const F32 time_delta = time - mLastTime;
    mLastTime = time;
    return time_delta;
}

// Local space means "parameter space".
LLVector3 LLBreastMotion::toLocal(const LLVector3 &world_vector)
{
    LLVector3 local_vec(0,0,0);

    LLJoint *chest_joint = mChestState->getJoint();
    const LLQuaternion world_rot = chest_joint->getWorldRotation();

    // Cleavage
    LLVector3 breast_dir_world_vec = LLVector3(-1,0,0) * world_rot; // -1 b/c cleavage param changes opposite to direction
    breast_dir_world_vec.normalize();
    local_vec[0] = world_vector * breast_dir_world_vec;

    // Up-Down Bounce
    LLVector3 breast_up_dir_world_vec = LLVector3(0,0,1) * world_rot;
    breast_up_dir_world_vec.normalize();
    local_vec[1] = world_vector * breast_up_dir_world_vec;

    return local_vec;
}

LLVector3 LLBreastMotion::calculateVelocity_local(const F32 time_delta)
{
    LLJoint *chest_joint = mChestState->getJoint();
    const LLVector3 world_pos_pt = chest_joint->getWorldPosition();
    const LLQuaternion world_rot = chest_joint->getWorldRotation();
    const LLVector3 last_world_pos_pt = mCharLastPosition_world_pt;
    const LLVector3 char_velocity_world_vec = (world_pos_pt-last_world_pos_pt) / time_delta;
    const LLVector3 char_velocity_local_vec = toLocal(char_velocity_world_vec);

    return char_velocity_local_vec;
}

LLVector3 LLBreastMotion::calculateAcceleration_local(const LLVector3 &new_char_velocity_local_vec,
                                                      const F32 time_delta)
{
    LLVector3 char_acceleration_local_vec = new_char_velocity_local_vec - mCharLastVelocity_local_vec;

    char_acceleration_local_vec =
        char_acceleration_local_vec * 1.0/mBreastSmoothingParam +
        mCharLastAcceleration_local_vec * (mBreastSmoothingParam-1.0)/mBreastSmoothingParam;

    mCharLastAcceleration_local_vec = char_acceleration_local_vec;

    return char_acceleration_local_vec;
}

BOOL LLBreastMotion::onUpdate(F32 time, U8* joint_mask)
{
    // Skip if disabled globally.
    if (!gSavedSettings.getBOOL("AvatarPhysics"))
    {
        return TRUE;
    }

    // Higher LOD is better.  This controls the granularity
    // and frequency of updates for the motions.
    const F32 lod_factor = LLVOAvatar::sPhysicsLODFactor;
    if (lod_factor == 0)
    {
        return TRUE;
    }

    if (mCharacter->getSex() != SEX_FEMALE) return TRUE;
    const F32 time_delta = calculateTimeDelta();
    if (time_delta < .01 || time_delta > 10.0) return TRUE;


    ////////////////////////////////////////////////////////////////////////////////
    // Get all parameters and settings
    //

    mBreastMassParam = mCharacter->getVisualParamWeight("Breast_Physics_Mass");
    mBreastSmoothingParam = (U32)(mCharacter->getVisualParamWeight("Breast_Physics_Smoothing"));
    mBreastGravityParam = mCharacter->getVisualParamWeight("Breast_Physics_Gravity");

    mBreastSpringParam[0] = mCharacter->getVisualParamWeight("Breast_Physics_Side_Spring");
    mBreastGainParam[0] = mCharacter->getVisualParamWeight("Breast_Physics_Side_Gain");
    mBreastDampingParam[0] = mCharacter->getVisualParamWeight("Breast_Physics_Side_Damping");
    mBreastMaxVelocityParam[0] = mCharacter->getVisualParamWeight("Breast_Physics_Side_Max_Velocity");
    mBreastDragParam[0] = mCharacter->getVisualParamWeight("Breast_Physics_Side_Drag");

    mBreastSpringParam[1] = mCharacter->getVisualParamWeight("Breast_Physics_UpDown_Spring");
    mBreastGainParam[1] = mCharacter->getVisualParamWeight("Breast_Physics_UpDown_Gain");
    mBreastDampingParam[1] = mCharacter->getVisualParamWeight("Breast_Physics_UpDown_Damping");
    mBreastMaxVelocityParam[1] = mCharacter->getVisualParamWeight("Breast_Physics_UpDown_Max_Velocity");
    mBreastDragParam[1] = mCharacter->getVisualParamWeight("Breast_Physics_UpDown_Drag");


    // Get the current morph parameters.
    LLVector3 breast_user_local_pt(0,0,0);
    for (U32 i=0; i < N_PARAMS; i++)
    {
        if (mBreastParamsUser[i] != NULL)
        {
            breast_user_local_pt[i] = mBreastParamsUser[i]->getWeight();
        }
    }

    LLVector3 breast_current_local_pt = mBreastLastPosition_local_pt;

    //
    // End parameters and settings
    ////////////////////////////////////////////////////////////////////////////////


    ////////////////////////////////////////////////////////////////////////////////
    // Calculate velocity and acceleration in parameter space.
    //

    const LLVector3 char_velocity_local_vec = calculateVelocity_local(time_delta);
    const LLVector3 char_acceleration_local_vec = calculateAcceleration_local(char_velocity_local_vec, time_delta);
    mCharLastVelocity_local_vec = char_velocity_local_vec;

    LLJoint *chest_joint = mChestState->getJoint();
    mCharLastPosition_world_pt = chest_joint->getWorldPosition();

    //
    // End velocity and acceleration
    ////////////////////////////////////////////////////////////////////////////////


    ////////////////////////////////////////////////////////////////////////////////
    // Calculate the total force
    //

    // Spring force is a restoring force towards the original user-set breast position.
    // F = kx
    const LLVector3 spring_length_local = breast_current_local_pt-breast_user_local_pt;
    LLVector3 force_spring_local_vec = -spring_length_local; force_spring_local_vec *= mBreastSpringParam;

    // Acceleration is the force that comes from the change in velocity of the torso.
    // F = ma + mg
    LLVector3 force_accel_local_vec = char_acceleration_local_vec * mBreastMassParam;
    const LLVector3 force_gravity_local_vec = toLocal(LLVector3(0,0,1))* mBreastGravityParam * mBreastMassParam;
    force_accel_local_vec += force_gravity_local_vec;
    force_accel_local_vec *= mBreastGainParam;

    // Damping is a restoring force that opposes the current velocity.
    // F = -kv
    LLVector3 force_damping_local_vec = -mBreastDampingParam;
    force_damping_local_vec *= mBreastVelocity_local_vec;

    // Drag is a force imparted by velocity, intuitively it is similar to wind resistance.
    // F = .5v*v
    LLVector3 force_drag_local_vec = .5*char_velocity_local_vec;
    force_drag_local_vec *= char_velocity_local_vec;
    force_drag_local_vec *= mBreastDragParam[0];

    LLVector3 force_net_local_vec =
        force_accel_local_vec +
        force_gravity_local_vec +
        force_spring_local_vec +
        force_damping_local_vec +
        force_drag_local_vec;

    //
    // End total force
    ////////////////////////////////////////////////////////////////////////////////


    ////////////////////////////////////////////////////////////////////////////////
    // Calculate new params
    //

    // Calculate the new acceleration based on the net force.
    // a = F/m
    LLVector3 acceleration_local_vec = force_net_local_vec / mBreastMassParam;
    mBreastVelocity_local_vec += acceleration_local_vec;
    mBreastVelocity_local_vec.clamp(-mBreastMaxVelocityParam*100.0, mBreastMaxVelocityParam*100.0);

    // Temporary debugging setting to cause all avatars to move, for profiling purposes.
    if (gSavedSettings.getBOOL("AvatarPhysicsTest"))
    {
        mBreastVelocity_local_vec[0] = sin(mTimer.getElapsedTimeF32()*4.0)*5.0;
        mBreastVelocity_local_vec[1] = sin(mTimer.getElapsedTimeF32()*3.0)*5.0;
    }
    // Calculate the new parameters and clamp them to the min/max ranges.
    LLVector3 new_local_pt = breast_current_local_pt + mBreastVelocity_local_vec*time_delta;
    new_local_pt.clamp(mBreastParamsMin,mBreastParamsMax);

    // Set the new parameters.
    for (U32 i=0; i < 3; i++)
    {
        // If the param is disabled, just set the param to the user value.
        if (mBreastMaxVelocityParam[i] == 0)
        {
            new_local_pt[i] = breast_user_local_pt[i];
        }
        if (mBreastParamsDriven[i])
        {
            mCharacter->setVisualParamWeight(mBreastParamsDriven[i],
                                             new_local_pt[i]);
        }
    }

    mBreastLastPosition_local_pt = new_local_pt;

    //
    // End calculate new params
    ////////////////////////////////////////////////////////////////////////////////


    ////////////////////////////////////////////////////////////////////////////////
    // Conditionally update the visual params
    //

    // Updating the visual params (i.e. what the user sees) is fairly expensive.
    // So only update if the params have changed enough, and also take into account
    // the graphics LOD settings.

    // For non-self, if the avatar is small enough visually, then don't update.
    const BOOL is_self = (dynamic_cast<LLVOAvatarSelf *>(this) != NULL);
    if (!is_self)
    {
        const F32 area_for_max_settings = 0.0;
        const F32 area_for_min_settings = 1400.0;

        const F32 area_for_this_setting = area_for_max_settings + (area_for_min_settings-area_for_max_settings)*(1.0-lod_factor);
        const F32 pixel_area = fsqrtf(mCharacter->getPixelArea());
        if (pixel_area < area_for_this_setting)
        {
            return TRUE;
        }
    }

    // If the parameter hasn't changed enough, then don't update.
    LLVector3 position_diff = mBreastLastUpdatePosition_local_pt-new_local_pt;
    for (U32 i=0; i < 3; i++)
    {
        const F32 min_delta = (1.0-lod_factor)*(mBreastParamsMax[i]-mBreastParamsMin[i])/2.0;
        if (llabs(position_diff[i]) > min_delta)
        {
            mCharacter->updateVisualParams();
            mBreastLastUpdatePosition_local_pt = new_local_pt;
            return TRUE;
        }
    }

    //
    // End update visual params
    ////////////////////////////////////////////////////////////////////////////////

    return TRUE;
}