MassCenterMotionRemover.h

Go to the documentation of this file.

#ifndef SimTK_MASS_CENTER_MOTION_REMOVER_H_
#define SimTK_MASS_CENTER_MOTION_REMOVER_H_
/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core biosimulation toolkit originating from      *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org.               *
 *                                                                            *
 * Portions copyright (c) 2009 Stanford University and the Authors.           *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKsimbody.h"
#include "molmodel/internal/common.h"
#include "molmodel/internal/MolecularMechanicsSystem.h"

namespace SimTK {

class MassCenterMotionRemover 
:   public PeriodicEventHandler {
public:
    explicit MassCenterMotionRemover(const MolecularMechanicsSystem& system, 
                            Real adjustmentInterval = 1)   // ps
    :   PeriodicEventHandler(adjustmentInterval),
        system(system), linearMomOnly(false), 
        enableCOMCorrection(false), comTol(1), comPos(0)
    {
    }

    MassCenterMotionRemover& disableAngularMomentumRemoval(bool disable) 
    {   linearMomOnly = disable; return *this; }
    bool isAngularMomentumRemovalDisabled() const {return linearMomOnly;}

    MassCenterMotionRemover& enableMassCenterCorrection(bool enabled) 
    {   enableCOMCorrection = enabled; return *this; }
    bool isMassCenterCorrectionEnabled() const {return enableCOMCorrection;}

    MassCenterMotionRemover& 
    setDesiredMassCenterLocation(const Vec3& groundLocationInNm) 
    {   comPos = groundLocationInNm; return *this; }
    const Vec3& getDesiredMassCenterLocation() const {return comPos;}

    MassCenterMotionRemover& 
    setMassCenterLocationTolerance(Real toleranceInNm) {
        SimTK_APIARGCHECK1_ALWAYS(toleranceInNm > 0, 
            "MassCenterMotionRemover","setMassCenterLocationTolerance", 
            "Illegal mass center tolerance %g.", toleranceInNm);
        comTol = toleranceInNm; 
        return *this; 
    }
    Real getMassCenterLocationTolerance() const {return comTol;}

    void removeSystemMomentum(State& state) const
    {   system.removeSystemRigidBodyMomentum(state, linearMomOnly); }

    void correctMassCenterLocation(State& state) const 
    {   system.moveSystemMassCenter(state, comPos); }

    Real calcMassCenterError(const State& state) const
    {   return (system.calcSystemMassCenterLocation(state) - comPos).norm(); }

    // This is the concrete implementation of the handleEvent() virtual
    // method required by every EventHandler; don't call this directly,
    // use removeSystemMomentum() or correctMassCenterLocation() instead.
    void handleEvent(State& state, Real accuracy, 
                     const Vector& yWeights, const Vector& ooConstraintTols, 
                     Stage& lowestModified, bool& shouldTerminate) const
    {
        system.realize(state, Stage::Velocity);
        removeSystemMomentum(state); 
        lowestModified = Stage::Velocity; // tell integrator what we changed
        if (enableCOMCorrection && calcMassCenterError(state) > comTol) {
            correctMassCenterLocation(state);
            lowestModified = Stage::Position;
        }
    }

private:
    const MolecularMechanicsSystem& system;
    bool    linearMomOnly; // should we correct angular momentum?
    bool    enableCOMCorrection; // should we relocate the COM?
    Real    comTol;    // how far to let the COM drift (nm).
    Vec3    comPos;    // where the COM is expected to stay.
};

} // namespace SimTK

#endif // SimTK_MASS_CENTER_MOTION_REMOVER_H_