[simspark-cvs] simspark/spark/oxygen/physicsserver angularmotor.cpp,NONE,1.1 angularmotor.h,NONE,1.1

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/simspark/simspark/spark/oxygen/physicsserver
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14301/physicsserver

Added Files:
	angularmotor.cpp angularmotor.h angularmotor_c.cpp 
	balljoint.cpp balljoint.h balljoint_c.cpp body.cpp body.h 
	body_c.cpp bodycontroller.cpp bodycontroller.h 
	bodycontroller_c.cpp boxcollider.cpp boxcollider.h 
	boxcollider_c.cpp ccylindercollider.cpp ccylindercollider.h 
	ccylindercollider_c.cpp collider.cpp collider.h collider_c.cpp 
	collisionhandler.cpp collisionhandler.h collisionhandler_c.cpp 
	contactjointhandler.cpp contactjointhandler.h 
	contactjointhandler_c.cpp dragcontroller.cpp dragcontroller.h 
	dragcontroller_c.cpp fixedjoint.cpp fixedjoint.h 
	fixedjoint_c.cpp hinge2joint.cpp hinge2joint.h 
	hinge2joint_c.cpp hingejoint.cpp hingejoint.h hingejoint_c.cpp 
	joint.cpp joint.h joint_c.cpp odeobject.cpp odeobject.h 
	odeobject_c.cpp physicsserver.cpp physicsserver.h 
	physicsserver_c.cpp planecollider.cpp planecollider.h 
	planecollider_c.cpp raycollider.cpp raycollider.h 
	raycollider_c.cpp recorderhandler.cpp recorderhandler.h 
	recorderhandler_c.cpp sliderjoint.cpp sliderjoint.h 
	sliderjoint_c.cpp space.cpp space.h space_c.cpp 
	spherecollider.cpp spherecollider.h spherecollider_c.cpp 
	universaljoint.cpp universaljoint.h universaljoint_c.cpp 
	velocitycontroller.cpp velocitycontroller.h 
	velocitycontroller_c.cpp world.cpp world.h world_c.cpp 
Log Message:

--- NEW FILE: recorderhandler.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: recorderhandler.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "recorderhandler.h"
#include "collider.h"

using namespace oxygen;
using namespace boost;

void
RecorderHandler::HandleCollision
(boost::shared_ptr<Collider> collidee, dContact& /*contact*/)
{
    mCollisionSet.insert(weak_ptr<Collider>(collidee));
}

void
RecorderHandler::Clear()
{
    mCollisionSet.clear();
}

void
RecorderHandler::GetParentsSupportingClass
(const std::string &name, TParentList &list)
{
    for (
         RecorderHandler::TCollisionSet::const_iterator iter = mCollisionSet.begin();
         iter != mCollisionSet.end();
         ++iter
         )
        {
            shared_ptr<Collider> collidee = make_shared(*iter);
            if (collidee.get() == 0)
                {
                    continue;
                }

            weak_ptr<Node> parent =
                collidee->GetParentSupportingClass(name);

            if (! parent.expired())
                {
                    list.push_back(parent);
                }
        }
}

--- NEW FILE: recorderhandler.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: recorderhandler.h,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef OXYGEN_RECORDERHANDLER_H
#define OXYGEN_COLLISIONRECORDER_H

#include "collisionhandler.h"
#include <set>

namespace oxygen
{
/** \class RecorderHandler is a CollisionHandler that accumulates
     collision information of the Collider it belongs to. It is the
     resonsibility of the user to reset the recorder.
*/
class RecorderHandler : public CollisionHandler
{
public:
    typedef std::set<boost::weak_ptr<Collider> > TCollisionSet;
    typedef std::list<boost::weak_ptr<Node> > TParentList;

public:
    RecorderHandler() : CollisionHandler() {};
    virtual ~RecorderHandler() {};

    /** stores the collidee into the internal CollisionSet

    \param collidee is the geom ID of the colliders collision
    partner

    \param holds the contact points between the two affected geoms
    as returned from the ODE dCollide function
    */
    virtual void HandleCollision
    (boost::shared_ptr<Collider> collidee, dContact& contact);

    /** removes all stored collisions from the recorder */
    void Clear();

    /** constructs a list of nodes that are parent nodes of the stored
        collidee nodes and are an instance of or are derived from the
        given class.

        \param name is the name of the class the parent nodes must
        support

        \param list is the list that receives the parent nodes
     */
    void GetParentsSupportingClass(const std::string &name, TParentList &list);

protected:
    TCollisionSet mCollisionSet;
};

    DECLARE_CLASS(RecorderHandler);

} //namespace oxygen

#endif // OXYGEN_COLLISIONRECORDER_H

--- NEW FILE: physicsserver.cpp ---
/* -*- mode: c++ -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: physicsserver.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "physicsserver.h"
#include <ode/ode.h>

using namespace oxygen;

PhysicsServer::PhysicsServer()
{
}

PhysicsServer::~PhysicsServer()
{
    // release memory associated with ode
    dCloseODE();
}

--- NEW FILE: contactjointhandler.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: contactjointhandler.h,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef OXYGEN_CONTACTJOINTHANDLER_H
#define OXYGEN_CONTACTJOINTHANDLER_H

#include "collisionhandler.h"

namespace oxygen
{

/** \class ContactJointHandler is a CollisionHandler that creates an
    ODE contact joint between the two bodies associated with the two
    affected collision geoms.
 */
class ContactJointHandler : public CollisionHandler
{
public:
    ContactJointHandler();
    virtual ~ContactJointHandler();

    /** Check if the collidee also has a ContactJoint handler
        registered to it. If yes, create a contact joint between the
        bodies corresponding to our Collider and the collidee's geom
        using the surface parameters stored in the mSurfaceParameter
        member.

        \param collidee is the geom ID of the colliders collision
        partner

        \param holds the contact points between the two affected geoms
        as returned from the ODE dCollide function
    */
    virtual void HandleCollision
    (boost::shared_ptr<Collider> collidee, dContact& contact);

    /** the ContactJointHandler is not a symmetric handler. See
        CollisionHandler::IsSymmetricHandler for an explanation
    */
    virtual bool IsSymmetricHandler() { return false; }

    /** sets the surface parameters for the contact joints that the
        CollisionHandler creates
    */
    void SetSurfaceParameter(const dSurfaceParameters& surface);

    /** sets or resets a contact mode flag in the surface parameter*/
    void SetContactMode(int mode, bool set);

    /** sets or resets the dContactBounce mode flag */
    void SetContactBounceMode(bool set);

    /** sets the bounce value */
    void SetBounceValue(float bounce);

    /** sets the mininum incoming velocity necessary for bounce */
    void SetMinBounceVel(float vel);

    /** sets or resets the error reduction parameter (ERP) mode,
        useful to make surfaces soft
    */
    void SetContactSoftERPMode(bool set);

    /** sets the contact normal error reduction parameter (ERP) */
    void SetContactSoftERP(float erp);

    /** sets or resets the constraint force mixing mode (CFM), useful
        to make surfaces soft
    */
    void SetContactSoftCFMMode(bool set);

    /** sets the constraint force mixing parameter (CFM)
    */
    void SetContactSoftCFM(float cfm);

    /** sets or resets the force dependent contact slip mode (FDS)
     */
    void SetContactSlipMode (bool set);

    /** sets the force dependent slip (FDS)
     */
    void SetContactSlip(float slip);

    /** sets the Coulomb friction coefficient */
    void SetContactMu(float mu);

protected:
    f_inline float MixValues(const float v1, const float v2, const int n) const;

    void CalcSurfaceParam(dSurfaceParameters& surface,
                          const dSurfaceParameters& collideeParam);

protected:
    /** the ODE surface parameters of the created contact joint */
    dSurfaceParameters mSurfaceParameter;
};

DECLARE_CLASS(ContactJointHandler);

} //namespace oxygen

#endif // OXYGEN_CONTACTJOINTHANDLER_H

--- NEW FILE: body_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: body_c.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "body.h"

using namespace boost;
using namespace oxygen;
using namespace zeitgeist;
using namespace salt;

FUNCTION(Body,enable)
{
   obj->Enable();
   return true;
}

FUNCTION(Body,disable)
{
    obj->Disable();
    return true;
}

FUNCTION(Body,isEnabled)
{
    return obj->IsEnabled();
}

FUNCTION(Body,useGravity)
{
    bool inB;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(),inB))
        )
        {
            return false;
        }

    obj->UseGravity(inB);
    return true;
}

FUNCTION(Body,setMassParameters)
{
    float inMass; // total mass of the rigid body
    Vector3f inCenter; // mass center in body frame

    // 3x3 inerta tensor in body frame
    // [ I11(0) I12(1) I13(2) ]
    // [ I12(3) I22(4) I23(5) ]
    // [ I13(6) I23(7) I33(8) ]
    // float inI[9];

    if (
        (in.GetSize() < 11)
        )
        {
            return false;
        }

    ParameterList::TVector::const_iterator iter = in.begin();
    if  (
         (! in.AdvanceValue(iter,inMass)) ||
         (! in.AdvanceValue(iter,inCenter))
         )
        {
            return false;
        }

    dMass mass;
    mass.mass = inMass;
    mass.c[0] = inCenter[0];
    mass.c[1] = inCenter[1];
    mass.c[2] = inCenter[2];

    for (int i=0;i<9;++i)
        {
            if (! in.AdvanceValue(iter,mass.I[i]))
                {
                    return false;
                }
        }

    obj->SetMassParameters(mass);
    return true;
}

FUNCTION(Body,setMass)
{
    float inMass;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inMass))
         )
        {
            return false;
        }

        obj->SetMass(inMass);
        return true;
}

FUNCTION(Body,getMass)
{
    return obj->GetMass();
}

FUNCTION(Body,setSphere)
{
    float inDensity;
    float inRadius;

    if (
        (in.GetSize() != 2) ||
        (! in.GetValue(in[0],inDensity)) ||
        (! in.GetValue(in[1],inRadius))
        )
        {
            return false;
        }

    obj->SetSphere(inDensity,inRadius);
    return true;
}

FUNCTION(Body,setSphereTotal)
{
    float inMassTotal;
    float inRadius;

    if (
        (in.GetSize() != 2) ||
        (! in.GetValue(in[0],inMassTotal)) ||
        (! in.GetValue(in[1],inRadius))
        )
        {
            return false;
        }

    obj->SetSphereTotal(inMassTotal,inRadius);
    return true;
}

FUNCTION(Body,setBox)
{
    float inDensity;
    Vector3f inSize;

    if (
        (in.GetSize() <= 1) ||
        (! in.GetValue(in[0],inDensity)) ||
        (! in.GetValue(in[1],inSize))
        )
        {
            return false;
        }

    obj->SetBox(inDensity,inSize);
    return true;
}

FUNCTION(Body,setBoxTotal)
{
    float inMassTotal;
    Vector3f inSize;

    if (
        (in.GetSize() <= 1) ||
        (! in.GetValue(in[0],inMassTotal)) ||
        (! in.GetValue(in[1],inSize))
        )
        {
            return false;
        }

    obj->SetBoxTotal(inMassTotal,inSize);
    return true;
}

FUNCTION(Body,setCylinder)
{
    float inDensity;
    float inRadius;
    float inLength;

    if (
        (in.GetSize() != 3) ||
        (! in.GetValue(in[0],inDensity)) ||
        (! in.GetValue(in[1],inRadius)) ||
        (! in.GetValue(in[2],inLength))
        )
        {
            return false;
        }

    obj->SetCylinder(inDensity,inRadius,inLength);
    return true;
}

FUNCTION(Body,setCylinderTotal)
{
    float inMassTotal;
    float inRadius;
    float inLength;

    if (
        (in.GetSize() != 3) ||
        (! in.GetValue(in[0],inMassTotal)) ||
        (! in.GetValue(in[1],inRadius)) ||
        (! in.GetValue(in[2],inLength))
        )
        {
            return false;
        }

    obj->SetCylinderTotal(inMassTotal,inRadius,inLength);
    return true;
}

FUNCTION(Body,setCappedCylinder)
{
    float inDensity;
    float inRadius;
    float inLength;

    if (
        (in.GetSize() != 3) ||
        (! in.GetValue(in[0],inDensity)) ||
        (! in.GetValue(in[1],inRadius)) ||
        (! in.GetValue(in[2],inLength))
        )
        {
            return false;
        }

    obj->SetCappedCylinder(inDensity,inRadius,inLength);
    return true;
}

FUNCTION(Body,setCappedCylinderTotal)
{
    float inMassTotal;
    float inRadius;
    float inLength;

    if (
        (in.GetSize() != 3) ||
        (! in.GetValue(in[0],inMassTotal)) ||
        (! in.GetValue(in[1],inRadius)) ||
        (! in.GetValue(in[2],inLength))
        )
        {
            return false;
        }

    obj->SetCappedCylinderTotal(inMassTotal,inRadius,inLength);
    return true;
}

FUNCTION(Body,setVelocity)
{
    Vector3f inVel;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inVel))
        )
        {
            return false;
        }

    obj->SetVelocity(inVel);
    return true;
}

FUNCTION(Body,setAngularVelocity)
{
    Vector3f inVel;

    if (
        (in.GetSize() == 1) ||
        (! in.GetValue(in.begin(), inVel))
        )
        {
            return false;
        }

    obj->SetAngularVelocity(inVel);
    return true;
}

FUNCTION(Body,addForce)
{
    Vector3f inForce;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inForce))
        )
        {
            return false;
        }

    obj->AddForce(inForce);
    return true;
}

FUNCTION(Body,addTorque)
{
    Vector3f inTorque;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inTorque))
        )
        {
            return false;
        }

    obj->AddForce(inTorque);
    return true;
}

FUNCTION(Body,setPosition)
{
    Vector3f inPos;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inPos))
        )
        {
            return false;
        }

    obj->AddForce(inPos);
    return true;
}

void CLASS(Body)::DefineClass()
{
        DEFINE_BASECLASS(oxygen/ODEObject);
        DEFINE_FUNCTION(enable);
        DEFINE_FUNCTION(disable);
        DEFINE_FUNCTION(isEnabled);
        DEFINE_FUNCTION(useGravity);
        DEFINE_FUNCTION(setSphere);
        DEFINE_FUNCTION(setSphereTotal);
        DEFINE_FUNCTION(setBox);
        DEFINE_FUNCTION(setBoxTotal);
        DEFINE_FUNCTION(setCylinder);
        DEFINE_FUNCTION(setCylinderTotal);
        DEFINE_FUNCTION(setCappedCylinder);
        DEFINE_FUNCTION(setCappedCylinderTotal);
        DEFINE_FUNCTION(setMass);
        DEFINE_FUNCTION(getMass);
        DEFINE_FUNCTION(setVelocity);
        DEFINE_FUNCTION(setAngularVelocity);
        DEFINE_FUNCTION(addForce);
        DEFINE_FUNCTION(addTorque);
        DEFINE_FUNCTION(setPosition);
        DEFINE_FUNCTION(setMassParameters);
}

--- NEW FILE: spherecollider.cpp ---
/* -*- mode: c++ -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: spherecollider.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "spherecollider.h"

using namespace oxygen;
using namespace salt;

SphereCollider::SphereCollider() : Collider()
{
}

void
SphereCollider::SetRadius(float r)
{
    dGeomSphereSetRadius(mODEGeom, r);
}

float
SphereCollider::GetRadius() const
{
    return dGeomSphereGetRadius(mODEGeom);
}

bool
SphereCollider::ConstructInternal()
{
    if (! Collider::ConstructInternal())
    {
        return false;
    }

    // create a unit sphere
    mODEGeom = dCreateSphere(0, 1.0f);

    return (mODEGeom != 0);
}

float
SphereCollider::GetPointDepth(const Vector3f& pos)
{
  Vector3f worldPos(GetWorldTransform() * pos);
  return dGeomSpherePointDepth
    (mODEGeom,worldPos[0],worldPos[1],worldPos[2]);
}

--- NEW FILE: collider.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: collider.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "collider.h"
#include "space.h"
#include "body.h"
#include "collisionhandler.h"
#include <oxygen/sceneserver/scene.h>
#include <zeitgeist/logserver/logserver.h>

using namespace oxygen;
using namespace salt;
using namespace boost;
using namespace std;

Collider::Collider() : ODEObject(), mODEGeom(0)
{
}

Collider::~Collider()
{
    if (mODEGeom)
        {
            dGeomDestroy(mODEGeom);
            mODEGeom = 0;
        }
}

void Collider::OnLink()
{
    ODEObject::OnLink();

    if (mODEGeom == 0)
        {
            return;
        }

    // if we have a space add the geom to it
    dSpaceID space = GetSpaceID();
    if (
        (space) &&
        (! dSpaceQuery(space, mODEGeom))
        )
        {
            dGeomSetData(mODEGeom, this);
            dSpaceAdd(space, mODEGeom);
        }

    // if there is a Body below our parent, link to it
    shared_ptr<Body> body = shared_static_cast<Body>
        (make_shared(GetParent())->GetChildOfClass("Body"));

    if (body.get() != 0)
        {
            dGeomSetBody (mODEGeom, body->GetODEBody());
        } else
            {
                // no body node found, setup initial position and
                // orientation identical to the parent node
                SetRotation(GetWorldTransform());
                SetPosition(Vector3f(0,0,0));
            }
}

void Collider::OnUnlink()
{
    ODEObject::OnUnlink();

    // remove collision geometry from space
    dSpaceID space = GetSpaceID();

    if (
        (mODEGeom == 0) ||
        (space == 0)
        )
        {
            return;
        }

    if (
        (space) &&
        (dSpaceQuery(space, mODEGeom))
        )
        {
            dSpaceRemove(space, mODEGeom);
        }
}

void Collider::PrePhysicsUpdateInternal(float /*deltaTime*/)
{
    if (GetChildSupportingClass("CollisionHandler").get() == 0)
        {
            // for convenience we add a ContactJointHandler if no
            // other handler is registered. This behaviour covers the
            // majority of all use cases and eases the creation of
            // Colliders.
            AddCollisionHandler("oxygen/ContactJointHandler");
        }
}

dGeomID Collider::GetODEGeom()
{
    return mODEGeom;
}

bool Collider::AddCollisionHandler(const std::string& handlerName)
{
    GetCore()->New(handlerName);

    shared_ptr<CollisionHandler> handler =
        shared_dynamic_cast<CollisionHandler>(GetCore()->New(handlerName));

    if (handler.get() == 0)
        {
            GetLog()->Error()
                << "ERROR: (Collider) Cannot create CollisionHandler "
                << handlerName << "\n";
            return false;
        }

    return AddChildReference(handler);
}

void Collider::OnCollision (boost::shared_ptr<Collider> collidee,
                            dContact& contact, ECollisionType type)

{
    TLeafList handlers;
    ListChildrenSupportingClass<CollisionHandler>(handlers);

    for (
         TLeafList::iterator iter = handlers.begin();
         iter != handlers.end();
         ++iter
         )
        {
            shared_ptr<CollisionHandler> handler =
                shared_static_cast<CollisionHandler>(*iter);

            if (
                (type == CT_SYMMETRIC) &&
                (! handler->IsSymmetricHandler())
                )
                {
                    continue;
                }

            handler->HandleCollision(collidee, contact);
        }
}

shared_ptr<Collider> Collider::GetCollider(dGeomID id)
{
    if (id == 0)
        {
            return shared_ptr<Collider>();
        }

    Collider* collPtr =
        static_cast<Collider*>(dGeomGetData(id));

    if (collPtr == 0)
        {
            // we cannot use the logserver here
            cerr << "ERROR: (Collider) no Collider found for dGeomID "
                 << id << "\n";
            return shared_ptr<Collider>();
        }

    shared_ptr<Collider> collider = shared_static_cast<Collider>
        (make_shared(collPtr->GetSelf()));

    if (collider.get() == 0)
        {
            // we cannot use the logserver here
            cerr << "ERROR: (Collider) got no shared_ptr for dGeomID "
                 << id << "\n";
        }

    return collider;
}

void Collider::SetRotation(const Matrix& rot)
{
    dMatrix3 m;
    ConvertRotationMatrix(rot,m);
    dGeomSetRotation(mODEGeom,m);
}

void Collider::SetPosition(const Vector3f& pos)
{
    Vector3f globalPos(GetWorldTransform() * pos);
    dGeomSetPosition (mODEGeom, globalPos[0], globalPos[1], globalPos[2]);
}

bool Collider::Intersects(boost::shared_ptr<Collider> collider)
{
    if (
        (mODEGeom == 0) ||
        (collider.get() == 0)
        )
        {
            return false;
        }

    dContactGeom contact;

    return dCollide
        (
         mODEGeom,
         collider->GetODEGeom(),
         1, /* ask for at most one collision point */
         &contact,
         sizeof(contact)
         ) > 0;
}

--- NEW FILE: universaljoint_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: universaljoint_c.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "universaljoint.h"

using namespace boost;
using namespace oxygen;
using namespace salt;

FUNCTION(UniversalJoint,setAnchor)
{
    Vector3f inAnchor;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inAnchor))
        )
        {
            return false;
        }

    obj->SetAnchor(inAnchor);
    return true;
}

FUNCTION(UniversalJoint,getAngle)
{
    int inAxisIndex;

    if (
        (in.GetSize() != 0) ||
        (! in.GetValue(in.begin(), inAxisIndex))
        )
        {
            return false;
        }

    obj->GetAngle(static_cast<Joint::EAxisIndex>(inAxisIndex));
    return true;
}

FUNCTION(UniversalJoint,getAngleRate)
{
    int inAxisIndex;

    if (
        (in.GetSize() != 0) ||
        (! in.GetValue(in.begin(), inAxisIndex))
         )
    {
        return false;
    }

    obj->GetAngleRate(static_cast<Joint::EAxisIndex>(inAxisIndex));
    return true;
}

void CLASS(UniversalJoint)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Joint);
    DEFINE_FUNCTION(setAnchor);
    DEFINE_FUNCTION(getAngle);
    DEFINE_FUNCTION(getAngleRate);
}

--- NEW FILE: angularmotor.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: angularmotor.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "angularmotor.h"
#include <zeitgeist/logserver/logserver.h>

using namespace oxygen;
using namespace boost;
using namespace salt;

AngularMotor::AngularMotor() : Joint()
{
}

AngularMotor::~AngularMotor()
{
}

void AngularMotor::OnLink()
{
    dWorldID world = GetWorldID();
    if (world == 0)
        {
            return;
        }

    mODEJoint = dJointCreateAMotor(world, 0);
}

void AngularMotor::SetMode(EMotorMode mode)
{
    dJointSetAMotorMode(mODEJoint,mode);
}

AngularMotor::EMotorMode AngularMotor::GetMode()
{
    return static_cast<EMotorMode>(dJointGetAMotorMode(mODEJoint));
}

void AngularMotor::SetNumAxes(int num)
{
    if (
        (num < 0) ||
        (num > 3)
        )
        {
            return;
        }

    dJointSetAMotorNumAxes(mODEJoint, num);
}

int AngularMotor::GetNumAxes()
{
    return dJointGetAMotorNumAxes(mODEJoint);
}

void AngularMotor::SetMotorAxis(EAxisIndex idx, EAxisAnchor anchor,
                                const salt::Vector3f& axis)
{
    Vector3f globalAxis = GetWorldTransform() * axis;
    dJointSetAMotorAxis (mODEJoint, idx, anchor,
                         globalAxis[0], globalAxis[1], globalAxis[2]);
}

AngularMotor::EAxisAnchor AngularMotor::GetAxisAnchor(EAxisIndex idx)
{
    return static_cast<EAxisAnchor>(dJointGetAMotorAxisRel (mODEJoint, idx));
}

Vector3f AngularMotor::GetMotorAxis(EAxisIndex idx)
{
    dVector3 dAxis;
    dJointGetAMotorAxis(mODEJoint,idx,dAxis);
    return Vector3f(dAxis[0],dAxis[1],dAxis[2]);
}

void AngularMotor::SetAxisAngle(EAxisIndex idx, float degAngle)
{
    dJointSetAMotorAngle(mODEJoint, idx, gDegToRad(degAngle));
}

float AngularMotor::GetAxisAngle(EAxisIndex idx)
{
    return gRadToDeg(dJointGetAMotorAngle(mODEJoint, idx));
}

float AngularMotor::GetAxisAngleRate(EAxisIndex idx)
{
    return gRadToDeg(dJointGetAMotorAngleRate(mODEJoint,idx));
}

void AngularMotor::SetParameter(int parameter, float value)
{
    dJointSetAMotorParam(mODEJoint, parameter, value);
}

float AngularMotor::GetParameter(int parameter)
{
    return dJointGetAMotorParam(mODEJoint, parameter);
}

--- NEW FILE: balljoint_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: balljoint_c.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "balljoint.h"

using namespace boost;
using namespace oxygen;
using namespace salt;

FUNCTION(BallJoint,setAnchor)
{
    Vector3f inAnchor;

    if (
        (in.GetSize() == 0) ||
        (! in.GetValue(in.begin(), inAnchor))
        )
        {
            return false;
        }

    obj->SetAnchor(inAnchor);
    return true;
}

void CLASS(BallJoint)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Joint);
    DEFINE_FUNCTION(setAnchor);
}

--- NEW FILE: contactjointhandler.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: contactjointhandler.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "contactjointhandler.h"
#include "collider.h"
#include "world.h"
#include "space.h"

#include <zeitgeist/logserver/logserver.h>

using namespace oxygen;
using namespace boost;

ContactJointHandler::ContactJointHandler() : CollisionHandler()
{
    // set up default contact surface parameters
    mSurfaceParameter.mode = dContactBounce;
    mSurfaceParameter.mu = dInfinity;
    mSurfaceParameter.bounce = 0.8f;
    mSurfaceParameter.bounce_vel = 2.0f;
}

ContactJointHandler::~ContactJointHandler()
{
}

float
ContactJointHandler::MixValues(const float v1, const float v2, const int n) const
{
    switch(n)
        {
        default:
        case 0:
            // undefined, default 0
            return 0.0f;

        case 1:
            // first one defined
            return v1;

        case 2:
            // second one defined
            return v2;

        case 3:
            // both defined, return average
            return (v1 + v2) / 2.0f;
        }
}

void
ContactJointHandler::CalcSurfaceParam(dSurfaceParameters& surface,
                                      const dSurfaceParameters& collideeParam)
{
    // init surface
    surface.mode = 0;

    // calculate average mu; mu can be dInfinity, so first multiply with
    // 0.5 and the sum up to avoid a range error
    surface.mu = mSurfaceParameter.mu*0.5f + collideeParam.mu*0.5f;

    // soft cfm
    const int nCfm =
        ((mSurfaceParameter.mode & dContactSoftCFM) ? 1:0) +
        ((collideeParam.mode & dContactSoftCFM) ? 2:0);

    if (nCfm>0)
        {
            surface.soft_cfm = MixValues
                (mSurfaceParameter.soft_cfm, collideeParam.soft_cfm, nCfm);
            surface.mode |= dContactSoftCFM;
        }

    // soft_erp
    const int nErp =
        ((mSurfaceParameter.mode & dContactSoftERP) ? 1:0) +
        ((collideeParam.mode & dContactSoftERP) ? 2:0);

    if (nErp>0)
        {
            surface.soft_erp = MixValues
                (mSurfaceParameter.soft_erp, collideeParam.soft_erp, nErp);
            surface.mode |= dContactSoftERP;
        }

    // bounce
    const int nBounce =
        ((mSurfaceParameter.mode & dContactBounce) ? 1:0) +
        ((collideeParam.mode & dContactBounce) ? 2:0);

    if (nBounce>0)
        {
            surface.bounce = MixValues
                (mSurfaceParameter.bounce, collideeParam.bounce, nBounce);

            surface.bounce_vel = MixValues
                (mSurfaceParameter.bounce_vel, collideeParam.bounce_vel, nBounce);

            surface.mode |= dContactBounce;
        }
}

void
ContactJointHandler::HandleCollision(shared_ptr<Collider> collidee, dContact& contact)
{
    if (
        (mCollider.get() == 0) ||
        (mWorld.get() == 0) ||
        (mSpace.get() == 0)
        )
        {
            return;
        }

    // to create a contact joint it we must have at least one body to
    // attach it to.
    dBodyID myBody = dGeomGetBody(mCollider->GetODEGeom());
    dBodyID collideeBody = dGeomGetBody(collidee->GetODEGeom());

    if (
        (myBody == 0) &&
        (collideeBody == 0)
        )
        {
            return;
        }

    // check if the collidee has a ContactJointHandler registered to it
    shared_ptr<ContactJointHandler> handler =
        collidee->FindChildSupportingClass<ContactJointHandler>();

    if (handler.get() == 0)
        {
            return;
        }

    // calculate the resulting surface parameters
    CalcSurfaceParam(contact.surface,handler->mSurfaceParameter);

    // create the contact joint and attach it to the body
    dJointID joint = dJointCreateContact
        (mWorld->GetODEWorld(), mSpace->GetODEJointGroup(), &contact);

    dJointAttach (joint, myBody, collideeBody);
}

void
ContactJointHandler::SetSurfaceParameter(const dSurfaceParameters& surface)
{
    mSurfaceParameter = surface;
}

void
ContactJointHandler::SetContactMode(int mode, bool set)
{
    if (set)
        {
            mSurfaceParameter.mode |= mode;
        } else
            {
                mSurfaceParameter.mode &= ~mode;
            }
}

void
ContactJointHandler::SetContactBounceMode(bool set)
{
    SetContactMode(dContactBounce,set);
}

void
ContactJointHandler::SetMinBounceVel(float vel)
{
    mSurfaceParameter.bounce_vel = std::max<float>(0.0f,vel);
}

void
ContactJointHandler::SetBounceValue(float bounce)
{
    mSurfaceParameter.bounce = std::max<float>(0.0f,bounce);
}

void
ContactJointHandler::SetContactSoftERPMode(bool set)
{
    SetContactMode(dContactSoftERP,set);
}

void
ContactJointHandler::SetContactSoftERP(float erp)
{
    salt::gClamp(erp,0.0f,1.0f);
    mSurfaceParameter.soft_erp = erp;
}

void
ContactJointHandler::SetContactSoftCFMMode(bool set)
{
    SetContactMode(dContactSoftCFM,set);
}

void
ContactJointHandler::SetContactSoftCFM(float cfm)
{
    mSurfaceParameter.soft_cfm = std::max<float>(0.0f,cfm);
}

void ContactJointHandler::SetContactSlipMode (bool set)
{
    SetContactMode(dContactSlip1,set);
    SetContactMode(dContactSlip2,set);
}

void ContactJointHandler::SetContactSlip(float slip)
{
    mSurfaceParameter.slip1 = slip;
    mSurfaceParameter.slip2 = slip;
}

void ContactJointHandler::SetContactMu(float mu)
{
    mSurfaceParameter.mu = mu;
}

--- NEW FILE: joint.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: joint.cpp,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include <zeitgeist/logserver/logserver.h>
#include "joint.h"
#include "body.h"

using namespace oxygen;
using namespace boost;
using namespace std;
using namespace salt;

Joint::Joint() : ODEObject(), mODEJoint(0)
{
}

Joint::~Joint()
{
    EnableFeedback(false);
    if (mODEJoint)
        {
            dJointDestroy(mODEJoint);
            mODEJoint = 0;
        }
}

void Joint::OnLink()
{
    if (mODEJoint == 0)
        {
            return;
        }

    dJointSetData(mODEJoint, this);
}

shared_ptr<Joint> Joint::GetJoint(dJointID id)
{
    if (id == 0)
        {
            return shared_ptr<Joint>();
        }

    Joint* jointPtr =
        static_cast<Joint*>(dJointGetData(id));

    if (jointPtr == 0)
        {
            // we cannot use the logserver here
            cerr << "ERROR: (Joint) no joint found for dJointID "
                 << id << "\n";
            return shared_ptr<Joint>();
        }

    shared_ptr<Joint> joint = shared_static_cast<Joint>
        (make_shared(jointPtr->GetSelf()));

    if (joint.get() == 0)
        {
            // we cannot use the logserver here
            cerr << "ERROR: (Joint) got no shared_ptr for dJointID "
                 << id << "\n";
        }

    return joint;
}

void Joint::Attach(shared_ptr<Body> body1, shared_ptr<Body> body2)
{
    if (mODEJoint == 0)
        {
            GetLog()->Error()
                << "(Joint) ERROR: Attach called with uninitialized ODE joint\n";
            return;
        }

    const dBodyID id1 = (body1.get() == 0) ? 0 : body1->GetODEBody();
    const dBodyID id2 = (body2.get() == 0) ? 0 : body2->GetODEBody();

    dJointAttach(mODEJoint, id1, id2);
}

shared_ptr<Body> Joint::GetBody(const std::string& path)
{
    if (path.empty())
        {
            return shared_ptr<Body>();
        }

    shared_ptr<Leaf> mySelf = shared_static_cast<Leaf>
        (make_shared(GetSelf()));

    shared_ptr<Leaf> leaf = GetCore()->Get(path,mySelf);

    if (leaf.get() == 0)
        {
            GetLog()->Error()
                << "(Joint) ERROR: cannot find node '"
                << path << "'\n";
            return shared_ptr<Body>();
        }

    shared_ptr<Body> body = shared_dynamic_cast<Body>(leaf);

    if (body.get() == 0)
        {
            GetLog()->Error()
                << "(Joint) ERROR: node '"
                << path << "' is not a Body node \n";
        }

    return body;
}

void Joint::Attach(const std::string& path1, const std::string& path2)
{
    shared_ptr<Body> body1 = GetBody(path1);
    shared_ptr<Body> body2 = GetBody(path2);

    Attach(body1,body2);
}

int Joint::GetType()
{
    return dJointGetType(mODEJoint);
}

boost::shared_ptr<Body> Joint::GetBody(EBodyIndex idx)
{
    return Body::GetBody(dJointGetBody(mODEJoint, idx));
}

bool Joint::AreConnected (shared_ptr<Body> body1, shared_ptr<Body> body2)
{
    if (
        (body1.get() == 0) ||
        (body2.get() == 0)
        )
        {
            return false;
        }

    return dAreConnected(body1->GetODEBody(),body2->GetODEBody());
}

bool Joint::AreConnectedExcluding (shared_ptr<Body> body1,
                                   shared_ptr<Body> body2,
                                   int joint_type)
{
    if (
        (body1.get() == 0) ||
        (body2.get() == 0)
        )
        {
            return false;
        }

    return dAreConnectedExcluding(body1->GetODEBody(),body2->GetODEBody(),
                                  joint_type);
}

void Joint::EnableFeedback(bool enable)
{
    if (enable)
        {
            if (mFeedback.get() == 0)
                {
                    mFeedback = shared_ptr<dJointFeedback>(new dJointFeedback());
                    memset(mFeedback.get(),0,sizeof(dJointFeedback));
                }
        } else
            {
                if (mFeedback.get() != 0)
                    {
                        mFeedback.reset();
                    }
            }

    dJointSetFeedback(mODEJoint,mFeedback.get());
}

bool Joint::FeedBackEnabled()
{
    return (dJointGetFeedback(mODEJoint) != 0);
}

Vector3f Joint::GetFeedbackForce(EBodyIndex idx)
{
    dJointFeedback* fb = mFeedback.get();
    if (fb == 0)
        {
            return Vector3f(0,0,0);
        }

    switch (idx)
        {
        case BI_FIRST :
            return Vector3f(
                            fb->f1[0],
                            fb->f1[1],
                            fb->f1[2]
                            );

        case BI_SECOND :
            return Vector3f(
                            fb->f2[0],
                            fb->f2[1],
                            fb->f2[2]
                            );

        default:
            return Vector3f(0,0,0);
        }
}

Vector3f Joint::GetFeedbackTorque(EBodyIndex idx)
{
    dJointFeedback* fb = mFeedback.get();
    if (fb == 0)
        {
            return Vector3f(0,0,0);
        }

    switch (idx)
        {
        case BI_FIRST :
            return Vector3f(
                            fb->t1[0],
                            fb->t1[1],
                            fb->t1[2]
                            );

        case BI_SECOND :
            return Vector3f(
                            fb->t2[0],
                            fb->t2[1],
                            fb->t2[2]
                            );

        default:
            return Vector3f(0,0,0);
        }
}

void Joint::SetBounce(EAxisIndex idx, float bounce)
{
    SetParameter(dParamBounce + (idx * dParamGroup),bounce);
}

float Joint::GetBounce(EAxisIndex idx)
{
    return GetParameter(dParamBounce + (idx * dParamGroup));
}

void Joint::SetLowStopPos(EAxisIndex idx, float pos)
{
    SetParameter(dParamLoStop + (idx * dParamGroup), pos);
}

float Joint::GetLowStopPos(EAxisIndex idx)
{
    return GetParameter(dParamLoStop + (idx * dParamGroup));
}

void Joint::SetHighStopPos(EAxisIndex idx, float pos)
{
    SetParameter(dParamHiStop + (idx * dParamGroup), pos);
}

float Joint::GetHighStopPos(EAxisIndex idx)
{
    return GetParameter(dParamHiStop + (idx * dParamGroup));
}

void Joint::SetLowStopDeg(EAxisIndex idx, float deg)
{
    SetParameter(dParamLoStop + (idx * dParamGroup), gDegToRad(deg));
}

float Joint::GetLowStopDeg(EAxisIndex idx)
{
    return gRadToDeg(GetParameter(dParamLoStop + (idx * dParamGroup)));
}

void Joint::SetHighStopDeg(EAxisIndex idx, float deg)
{
    SetParameter(dParamHiStop + (idx * dParamGroup), gDegToRad(deg));
}

float Joint::GetHighStopDeg(EAxisIndex idx)
{
    return gRadToDeg(GetParameter(dParamHiStop + (idx * dParamGroup)));
}

void Joint::SetCFM(EAxisIndex idx, float cfm)
{
    SetParameter(dParamCFM + (idx * dParamGroup), cfm);
}

float Joint::GetCFM(EAxisIndex idx)
{
    return GetParameter(dParamCFM + (idx * dParamGroup));
}

void Joint::SetStopCFM(EAxisIndex idx, float cfm)
{
    SetParameter(dParamStopCFM + (idx * dParamGroup), cfm);
}

float Joint::GetStopCFM(EAxisIndex idx)
{
    return GetParameter(dParamStopCFM + (idx * dParamGroup));
}

void Joint::SetStopERP(EAxisIndex idx, float erp)
{
    SetParameter(dParamStopERP + (idx * dParamGroup), erp);
}

float Joint::GetStopERP(EAxisIndex idx)
{
    return GetParameter(dParamStopERP + (idx * dParamGroup));
}

void Joint::SetSuspensionERP(EAxisIndex idx, float erp)
{
    SetParameter(dParamSuspensionERP + (idx * dParamGroup), erp);
}

float Joint::GetSuspensionERP(EAxisIndex idx)
{
    return GetParameter(dParamSuspensionERP + (idx * dParamGroup));
}

void Joint::SetSuspensionCFM(EAxisIndex idx, float cfm)
{
    SetParameter(dParamSuspensionCFM + (idx * dParamGroup), cfm);
}

float Joint::GetSuspensionCFM(EAxisIndex idx)
{
    return GetParameter(dParamSuspensionCFM + (idx * dParamGroup));
}

void Joint::SetLinearMotorVelocity(EAxisIndex idx, float vel)
{
    SetParameter(dParamVel + (idx * dParamGroup), vel);
}

float Joint::GetLinearMotorVelocity(EAxisIndex idx)
{
    return GetParameter(dParamVel + (idx * dParamGroup));
}

void Joint::SetAngularMotorVelocity(EAxisIndex idx, float deg)
{
    SetParameter(dParamVel + (idx * dParamGroup), gDegToRad(deg));
}

float Joint::GetAngularMotorVelocity(EAxisIndex idx)
{
    return gRadToDeg(GetParameter(dParamVel + (idx * dParamGroup)));
}

void Joint::SetMaxMotorForce(EAxisIndex idx, float f)
{
    SetParameter(dParamFMax + (idx * dParamGroup), f);
}

float Joint::GetMaxMotorForce(EAxisIndex idx)
{
    return GetParameter(dParamFMax + (idx * dParamGroup));
}

--- NEW FILE: odeobject.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: odeobject.h,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef OXYGEN_ODEOBJECT_H
#define OXYGEN_ODEOBJECT_H

#include <oxygen/sceneserver/basenode.h>
#include <ode/ode.h>

namespace oxygen
{
class Space;
class World;

/** ODEObject is the base of all classes encapsulating ODE concepts
 */
class ODEObject : public BaseNode
{
public:
    //
    // Functions
    //
    ODEObject() : BaseNode() {};
    virtual ~ODEObject() {};

protected:
    /** returns the world node */
    boost::shared_ptr<World> GetWorld();

    /** returns the space node */
    boost::shared_ptr<Space> GetSpace();

    /** returns the ODE world handle */
    dWorldID GetWorldID();

    /** returns the ODE space handle */
    dSpaceID GetSpaceID();

    /** converts the rotation part of a salt::Matrix to an ODE
        dMatrix3 */
    void ConvertRotationMatrix(const salt::Matrix& rot, dMatrix3& matrix);
};

DECLARE_ABSTRACTCLASS(ODEObject);

} //namespace oxygen

#endif //OXYGEN_ODEOBJECT_H

--- NEW FILE: ccylindercollider.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: ccylindercollider.h,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef OXYGEN_CCYLINDERCOLLIDER_H
#define OXYGEN_CCYLINDERCOLLIDER_H

#include "collider.h"

namespace oxygen
{

/** CCylinderCollider encapsulates an ODE capped cylinder geometry
    object. A capped cylinder is like a normal cylinder except it has
    half-sphere caps at its ends. This feature makes the internal
    collision detection code particularly fast and accurate.
 */
class CCylinderCollider : public Collider
{
    //
    // Functions
    //
public:
    CCylinderCollider();

    /** sets the parameters of the capped cylinder.

       \param radius is the radius of the caps, and of the cylinder itself
       \param length is the height of the cylinder, not counting the caps
    */
    void SetParams(float radius, float length);

    /** sets the radius of the capped cylinder */
    void SetRadius(float radius);

    /** sets the length of the capped cylinder */
    void SetLength(float length);

    /** gets the radius and the length of the capped cylinder */
    void GetParams(float& radius, float& length);

    /** returns the radius of the capped cylinder */
    float GetRadius();

    /** return the length of the capped cylinder */
    float GetLength();

    /** returns the depth of the given relative position in the
       managed capped cylinder geom. Points inside the geom will have
       positive depth, points outside it will have negative depth, and
       points on the surface will have zero depth.
     */
    float GetPointDepth(const salt::Vector3f& pos);

protected:
    /** constructs a default capped cylinder with an radius of 1 and a
        length of 1 */
    virtual bool ConstructInternal();
};

DECLARE_CLASS(CCylinderCollider);

} //namespace oxygen

#endif //OXYGEN_CCYLINDERCOLLIDER_H

--- NEW FILE: joint.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: joint.h,v 1.1 2005/12/05 21:16:49 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef OXYGEN_JOINT_H
#define OXYGEN_JOINT_H

#include "odeobject.h"

namespace oxygen
{
class Body;

/** \class Joint encapsulates an ODE joint object. A joint is a
    relationship (a constraint) that is enforced between two bodies so
    that they can only have certain positions and orientations
    relative to each other.

    Note that the joint geometry parameter setting functions should
    only be called after the joint has been attached to bodies, and
    those bodies have been correctly positioned, otherwise the joint
    may not be initialized correctly. If the joint is not already
    attached, these functions will do nothing.

    The default anchor for all joints is global (0,0,0). The default
    axis for all joints is global (1,0,0).

    There are no functions to set joint angles or positions (or their
    rates) directly, instead you must set the corresponding body
    positions and velocities.
 */
class Joint : public ODEObject
{
public:
    enum EBodyIndex
        {
            BI_FIRST = 0,
            BI_SECOND = 1
        };

    enum EAxisIndex
        {
            AI_FIRST = 0,
            AI_SECOND = 1,
            AI_THIRD = 2
        };

    Joint();
    virtual ~Joint();

    /** attaches the joint to some new bodies. If the joint is already
        attached, it will be detached from the old bodies first. To
        attach this joint to only one body, set body1 or body2 to
        null.  A null body refers to the static environment. Setting
        both bodies to zero puts the joint into "limbo", i.e. it will
        have no effect on the simulation.
    */
    virtual void Attach(boost::shared_ptr<Body> body1,
                        boost::shared_ptr<Body> body2);

    /** attaches the joint to some new bodies, that are given as path
        expressions. These path expressions are allowed to be relative
        to this joint node.
     */
    void Attach(const std::string& path1, const std::string& path2);

    /** returns one of the bodies that this joint connects, according
        to the given EBodyIndex
    */
    boost::shared_ptr<Body> GetBody(EBodyIndex idx);

    /** returns the Joint node corresponding to the given ODE joint */
    static boost::shared_ptr<Joint> GetJoint(dJointID id);

    /** returns the type of the managed ODE joint, possible return
        values are dJointTypeNone, dJointTypeBall, dJointTypeHinge,
        dJointTypeSlider, dJointTypeContact, dJointTypeUniversal,
        dJointTypeHinge2, dJointTypeFixed or dJointTypeAMotor.
     */
    int GetType();

    /** returns true if the two given bodies are connected by a
        joint */
    static bool AreConnected (boost::shared_ptr<Body> body1,
                              boost::shared_ptr<Body> body2);

    /** returns true if the two given bodies are connected together by
        a joint that does not have the given joint type. For possible
        joint type constants see GetType()
     */
    static bool AreConnectedExcluding (boost::shared_ptr<Body> body1,
                                       boost::shared_ptr<Body> body2,
                                       int joint_type);

    /** during the world time step, the forces that are applied by
        each joint are added directly to the joined bodies, and the
        user normally has no way of telling which joint contributed
        how much force. If this information is desired the joint can
        be enabled to collect feedback information. By default a joint
        does not collect any feedback information.
     */
    void EnableFeedback(bool enable);

    /** returns true if the joint is set to collect feedback
        information */
    bool FeedBackEnabled();

    /** queries the force that the joint applied to one body attached
        to it during the last timestep.
    */
    salt::Vector3f GetFeedbackForce(EBodyIndex idx);

    /** queries the torque that the joint applied to one body attached
        to it during the last timestep.
    */
    salt::Vector3f GetFeedbackTorque(EBodyIndex idx);

    /** sets the bouncyness of the stops. This is a restitution
        parameter in the range 0..1. 0 means the stops are not bouncy
        at all, 1 means maximum bouncyness.
    */
    void SetBounce(EAxisIndex idx, float bounce);

    /** returns the bouncyness of the stops */
    float GetBounce(EAxisIndex idx);

    /** sets the low stop angle in degrees, this stop must be greater
        than -180 to be effective
    */
    void SetLowStopDeg(EAxisIndex idx, float deg);

    /** returns the low stop angle in degrees */
    float GetLowStopDeg(EAxisIndex idx);

    /** sets the high stop angle in degrees, this stop must be less
        than +180 to be effective
    */
    void SetHighStopDeg(EAxisIndex idx, float deg);

    /** returns the high stop angle in degrees */
    float GetHighStopDeg(EAxisIndex idx);

    /** sets the low stop position */
    void SetLowStopPos(EAxisIndex idx, float deg);

    /** returns the low stop position */
    float GetLowStopPos(EAxisIndex idx);

    /** sets the high stop position */
    void SetHighStopPos(EAxisIndex idx, float deg);

    /** returns the high stop position */
    float GetHighStopPos(EAxisIndex idx);

    /** the constraint force mixing (CFM) value used when not at a
        stop */
    void SetCFM(EAxisIndex idx, float cfm);

    /** returns the constraint force mixing value used when not a a
        stop */
    float GetCFM(EAxisIndex idx);

    /** sets the constraint force mixing (CFM) value used by the
        stops. Together with the ERP value this can be used to get
        spongy or soft stops. This is intended for unpowered joints,
        it does not really work as expected when a powered joint
        reaches its limit.
    */
    void SetStopCFM(EAxisIndex idx, float cfm);

    /** returns the constraint force mixing value used by the stops */
    float GetStopCFM(EAxisIndex idx);

    /** sets the error reduction parameter (ERP) used by the stops. */
    void SetStopERP(EAxisIndex idx, float erp);

    /** returns the error reduction parameter used by the stops */
    float GetStopERP(EAxisIndex idx);

    /** sets the suspension error reduction parameter (ERP). As of ode
        0.039 this is only implemented on the hinge-2 joint.
    */
    void SetSuspensionERP(EAxisIndex idx, float erp);

    /** returns the suspension error reduction parameter (ERP). As of
        ode 0.039 this is only implemented on the hinge-2 joint.
    */
    float GetSuspensionERP(EAxisIndex idx);

    /** sets the suspension constraint force mixing value. As of ode
        0.039 this is only implemented on the hinge-2 joint.
    */
    void SetSuspensionCFM(EAxisIndex idx, float cfm);

    /** returns the suspension constraint force mixing value. As of
        ode 0.039 this is only implemented on the hinge-2 joint.
    */
    float GetSuspensionCFM(EAxisIndex idx...

[truncated message content]