SpringMotor.cpp

Go to the documentation of this file.

/*************************************************************************
 *                                                                       *
 * Open Physics Abstraction Layer                                        *
 * Copyright (C) 2004-2005                                               *
 * Alan Fischer  alan.fischer@gmail.com                                  *
 * Andres Reinot  andres@reinot.com                                      *
 * Tyler Streeter  tylerstreeter@gmail.com                               *
 * All rights reserved.                                                  *
 * Web: opal.sourceforge.net                                             *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file license-LGPL.txt.                                          *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file license-BSD.txt.                                       *
 *                                                                       *
 * 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 files    *
 * license-LGPL.txt and license-BSD.txt for more details.                *
 *                                                                       *
 *************************************************************************/

#include "SpringMotor.h"
#include "Solid.h"

namespace opal
{
        SpringMotor::SpringMotor()
        : Motor()
        {
                // "mData" will be initialized in its own constructor.
        }

        SpringMotor::~SpringMotor()
        {
        }

        void SpringMotor::init(const SpringMotorData& data)
        {
                Motor::init();
                mData = data;
        }

        const SpringMotorData& SpringMotor::getData()const
        {
                return mData;
        }

        MotorType SpringMotor::getType()const
        {
                return mData.getType();
        }

        void SpringMotor::setName(const std::string& name)
        {
                mData.name = name;
        }

        const std::string& SpringMotor::getName()const
        {
                return mData.name;
        }

        bool SpringMotor::isEnabled()const
        {
                return mData.enabled;
        }

        void SpringMotor::setEnabled(bool e)
        {
                //if (!mInitCalled)
                //{
                //      return;
                //}

                mData.enabled = e;
        }

        void SpringMotor::internal_update()
        {
                // Note: this only applies to global position and orientation.

                if (mData.enabled && mData.solid)
                {
                        if (LINEAR_MODE == mData.mode || 
                                LINEAR_AND_ANGULAR_MODE == mData.mode)
                        {
                                Vec3r error = mData.desiredPos - getGlobalAttachPoint();
                                Force f;
                                f.singleStep = true;
                                f.type = GLOBAL_FORCE_AT_LOCAL_POS;
                                f.pos = mData.attachOffset;
                                f.vec = mData.linearKs * error - 
                                        mData.linearKd * mData.solid->getGlobalLinearVel();

                                // Scale the force vector by the Solid's mass.
                                f.vec *= mData.solid->getMass();
                                mData.solid->addForce(f);
                        }

                        if (ANGULAR_MODE == mData.mode || 
                                LINEAR_AND_ANGULAR_MODE == mData.mode)
                        {
                                // Find cross products of actual and desired forward, up, 
                                // and right vectors; these represent the orientation error.
                                Matrix44r transform = mData.solid->getTransform();
                                Vec3r actualForward = transform.getForward();
                                Vec3r actualUp = transform.getUp();
                                Vec3r actualRight = transform.getRight();
                                
                                if (0 != actualForward.lengthSquared())
                                {
                                        actualForward.normalize();
                                }
                                if (0 != actualUp.lengthSquared())
                                {
                                        actualUp.normalize();
                                }
                                if (0 != actualRight.lengthSquared())
                                {
                                        actualRight.normalize();
                                }

                                Vec3r forwardError = cross(mData.desiredForward, 
                                        actualForward);
                                Vec3r upError = cross(mData.desiredUp, actualUp);
                                Vec3r rightError = cross(mData.desiredRight, actualRight);

                                if (0 != forwardError.lengthSquared())
                                {
                                        forwardError.normalize();
                                }
                                if (0 != upError.lengthSquared())
                                {
                                        upError.normalize();
                                }
                                if (0 != rightError.lengthSquared())
                                {
                                        rightError.normalize();
                                }

                                // Scale error vectors by the magnitude of the angles.
                                real fangle = angleBetweenPreNorm(mData.desiredForward, 
                                        actualForward);
                                real uangle = angleBetweenPreNorm(mData.desiredUp, 
                                        actualUp);
                                real rangle = angleBetweenPreNorm(mData.desiredRight, 
                                        actualRight);

                                forwardError *= -fangle;
                                upError *= -uangle;
                                rightError *= -rangle;

                                // Average the vectors into one.
                                Vec3r errorAxis = (forwardError + upError + rightError) * 
                                        globals::OPAL_ONE_THIRD;

                                // Use the error vector to calculate torque.
                                Force f;
                                f.singleStep = true;
                                f.type = GLOBAL_TORQUE;
                                f.vec = mData.angularKs * errorAxis - mData.angularKd * 
                                        mData.solid->getGlobalAngularVel();

                                // Scale the torque vector by the Solid's intertia tensor.
                                f.vec = mData.solid->getInertiaTensor() * f.vec;
                                mData.solid->addForce(f);
                        }
                }
        }

        void SpringMotor::setLocalAttachOffset(const Point3r& offset)
        {
                mData.attachOffset = offset;
        }

        const Point3r& SpringMotor::getLocalAttachOffset()const
        {
                return mData.attachOffset;
        }

        void SpringMotor::setGlobalAttachPoint(const Point3r& p)
        {
                if (!mData.solid)
                {
                        OPAL_LOGGER("warning") << 
                                "opal::SpringMotor::setGlobalAttachPoint: Solid pointer is \
invalid.  Ignoring request." << std::endl;
                        return;
                }

                // Convert the global point to a local point offset from the Solid's 
                // transform.
                Matrix44r inv = mData.solid->getTransform();
                inv.fastInvert();
                mData.attachOffset = inv * p;
        }

        Point3r SpringMotor::getGlobalAttachPoint()const
        {
                if (!mData.solid)
                {
                        OPAL_LOGGER("warning") << 
                                "opal::SpringMotor::getGlobalAttachPoint: Solid pointer is \
invalid.  Returning (0,0,0)." << std::endl;
                        return Point3r();
                }

                // The global position is a combination of the Solid's global 
                // transform and the spring's local offset from the Solid's 
                // transform.
                Vec3r offset(mData.attachOffset[0], mData.attachOffset[1], 
                        mData.attachOffset[2]);
                Point3r globalPos = mData.solid->getPosition() + 
                        mData.solid->getTransform() * offset;
                
                return globalPos;
        }

        void SpringMotor::setDesiredTransform(const Matrix44r& transform)
        {
                mData.desiredPos = transform.getPosition();

                mData.desiredForward = transform.getForward();
                if (0 != mData.desiredForward.lengthSquared())
                {
                        mData.desiredForward.normalize();
                }

                mData.desiredUp = transform.getUp();
                if (0 != mData.desiredUp.lengthSquared())
                {
                        mData.desiredUp.normalize();
                }

                mData.desiredRight = transform.getRight();
                if (0 != mData.desiredRight.lengthSquared())
                {
                        mData.desiredRight.normalize();
                }
        }

        void SpringMotor::setDesiredPosition(const Point3r& pos)
        {
                mData.desiredPos = pos;
        }

        void SpringMotor::setDesiredOrientation(const Vec3r& forward, 
                const Vec3r& up, const Vec3r& right)
        {
                mData.desiredForward = forward;
                if (0 != mData.desiredForward.lengthSquared())
                {
                        mData.desiredForward.normalize();
                }

                mData.desiredUp = up;
                if (0 != mData.desiredUp.lengthSquared())
                {
                        mData.desiredUp.normalize();
                }

                mData.desiredRight = right;
                if (0 != mData.desiredRight.lengthSquared())
                {
                        mData.desiredRight.normalize();
                }
        }

        void SpringMotor::setLinearKd(real kd)
        {
                mData.linearKd = kd;
        }

        void SpringMotor::setLinearKs(real ks)
        {
                mData.linearKs = ks;
        }

        real SpringMotor::getLinearKd()const
        {
                return mData.linearKd;
        }

        real SpringMotor::getLinearKs()const
        {
                return mData.linearKs;
        }

        void SpringMotor::setAngularKd(real kd)
        {
                mData.angularKd = kd;
        }

        void SpringMotor::setAngularKs(real ks)
        {
                mData.angularKs = ks;
        }

        real SpringMotor::getAngularKd()const
        {
                return mData.angularKd;
        }

        real SpringMotor::getAngularKs()const
        {
                return mData.angularKs;
        }

        bool SpringMotor::internal_dependsOnSolid(Solid* s)
        {
                if (s == mData.solid)
                {
                        return true;
                }
                else
                {
                        return false;
                }
        }
}

---