#include "OgreOdePrecompiledHeaders.h"
#include "OgreOdeJoint.h"

#include "OgreOdeWorld.h"
#include "OgreOdeBody.h"
#include "OgreOdeGeometry.h"

using namespace OgreOde;
using namespace Ogre;

//------------------------------------------------------------------------------------------------
JointGroup::JointGroup(World *world):
_world(world)
{
	_joint_group = dJointGroupCreate(0); 
	_world->getJointGroupList().registerItem(this);
}
//------------------------------------------------------------------------------------------------
dJointGroupID JointGroup::getJointGroupID() const
{
	return _joint_group;
}

size_t JointGroup::getID()
{
	return (size_t)_joint_group;
}

void JointGroup::empty()
{
	dJointGroupEmpty(_joint_group); 
}

JointGroup::~JointGroup()
{
	_world->getJointGroupList().unregisterItem((size_t)_joint_group);
	dJointGroupDestroy(_joint_group); 
}

Joint::Joint(World *world, const JointGroup* group):
    _world(world)
{
}

void Joint::registerJoint()
{
	_world->getJointList().registerItem(this);
}

void Joint::setAnchor(const Ogre::Vector3& position)
{
}

void Joint::addTorque(Real torque,Real torque2,Real torque3)
{
}

void Joint::addForce(Real force,Real force2,Real force3)
{
}

const Ogre::Vector3& Joint::getAnchor()
{
	return Ogre::Vector3::ZERO;
}

const Ogre::Vector3& Joint::getAnchorError()
{
	return Ogre::Vector3::ZERO;
}

void Joint::setAxis(const Ogre::Vector3& axis)
{
}

const Ogre::Vector3& Joint::getAxis()
{
	return Ogre::Vector3::ZERO;
}

void Joint::setAdditionalAxis(const Ogre::Vector3& axis)
{
}

const Ogre::Vector3& Joint::getAdditionalAxis()
{
	return Ogre::Vector3::ZERO;
}

Real Joint::getAngle()
{
	return 0.0;
}

Real Joint::getAngleRate()
{
	return 0.0;
}

Real Joint::getPosition()
{
	return 0.0;
}

Real Joint::getPositionRate()
{
	return 0.0;
}

size_t Joint::getID()
{
	return (size_t)_joint;
}

Joint::~Joint()
{
	_world->getJointList().unregisterItem((size_t)_joint);
	dJointDestroy(_joint); 
}

dWorldID Joint::getWorldID()
{
	return _world->getWorldID();
}

dJointGroupID Joint::getJointGroupID(const JointGroup* group) const
{
	return ((group)?group->getJointGroupID():0);
}

dJointID Joint::getJointID()
{
	return _joint;
}

Joint::Type Joint::getType()
{
	return (Joint::Type)(dJointGetType(_joint));
}

Body* Joint::getFirstBody()
{
	dBodyID b = dJointGetBody(_joint, 0); 
	return ((Body*)dBodyGetData(b));
}

Body* Joint::getSecondBody()
{
	dBodyID b = dJointGetBody(_joint,1);
	return b ? ((Body*)dBodyGetData(b)) : 0; 
}

bool Joint::areConnected(const Body* body_a,const Body* body_b)
{
	return (dAreConnected(body_a->getBodyID(),body_b->getBodyID()))?true:false; 
}

bool Joint::areConnectedExcluding(const Body* body_a,const Body* body_b,Joint::Type joint_type)
{
	return (dAreConnectedExcluding(body_a->getBodyID(),body_b->getBodyID(),(int)joint_type))?true:false; 
}

void Joint::enableFeedback()
{
	dJointSetFeedback(_joint,&_feedback); 
}

void Joint::disableFeedback()
{
	dJointSetFeedback(_joint,0); 
}

void Joint::detach()
{
	dJointAttach(_joint,0,0); 
}

void Joint::attach(const Body* body)
{
	dJointAttach(_joint,body->getBodyID(),0); 
}

void Joint::attach(const Body* body_a,const Body* body_b)
{
	dJointAttach(_joint,body_a->getBodyID(),body_b->getBodyID()); 
}

const Ogre::Vector3& Joint::getFirstForce()
{
	assert(dJointGetFeedback(_joint) == &_feedback);

	_first_force.x = _feedback.f1[0];
	_first_force.y = _feedback.f1[1];
	_first_force.z = _feedback.f1[2];
	return _first_force;
}

const Ogre::Vector3& Joint::getFirstTorque()
{
	assert(dJointGetFeedback(_joint) == &_feedback);

	_first_torque.x = _feedback.t1[0];
	_first_torque.y = _feedback.t1[1];
	_first_torque.z = _feedback.t1[2];
	return _first_torque;
}

const Ogre::Vector3& Joint::getSecondForce()
{
	assert(dJointGetFeedback(_joint) == &_feedback);

	_second_force.x = _feedback.f2[0];
	_second_force.y = _feedback.f2[1];
	_second_force.z = _feedback.f2[2];
	return _second_force;
}

const Ogre::Vector3& Joint::getSecondTorque()
{
	assert(dJointGetFeedback(_joint) == &_feedback);

	_second_torque.x = _feedback.t2[0];
	_second_torque.y = _feedback.t2[1];
	_second_torque.z = _feedback.t2[2];
	return _second_torque;
}

void Joint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
}

Real Joint::getParameter(Joint::Parameter parameter,int axis)
{
	return 0.0;
}

BallJoint::BallJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateBall(getWorldID(),getJointGroupID(group));
	registerJoint();
}

void BallJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetBallAnchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}

const Ogre::Vector3& BallJoint::getAnchor()
{
	dVector3 result;
	dJointGetBallAnchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}

const Ogre::Vector3& BallJoint::getAnchorError()
{
	dVector3 result1,result2;
	dJointGetBallAnchor(_joint,result1); 
	dJointGetBallAnchor2(_joint,result2); 
	_anchor_error.x = (Real)(result1[0] - result2[0]);
	_anchor_error.y = (Real)(result1[1] - result2[1]);
	_anchor_error.z = (Real)(result1[2] - result2[2]);
	return _anchor_error;
}

void BallJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetBallParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value);  
}

Real BallJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetBallParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}


BallJoint::~BallJoint()
{
}

HingeJoint::HingeJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateHinge(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void HingeJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetHingeAnchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}

void HingeJoint::addTorque(Real torque,Real torque2,Real torque3)
{
	dJointAddHingeTorque(_joint,(dReal)torque);
}

const Ogre::Vector3& HingeJoint::getAnchor()
{
	dVector3 result;
	dJointGetHingeAnchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}

const Ogre::Vector3& HingeJoint::getAnchorError()
{
	dVector3 result1,result2;
	dJointGetHingeAnchor(_joint,result1); 
	dJointGetHingeAnchor2(_joint,result2); 
	_anchor_error.x = (Real)(result1[0] - result2[0]);
	_anchor_error.y = (Real)(result1[1] - result2[1]);
	_anchor_error.z = (Real)(result1[2] - result2[2]);
	return _anchor_error;
}

void HingeJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetHingeAxis(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& HingeJoint::getAxis()
{
	dVector3 result;
	dJointGetHingeAxis(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

Real HingeJoint::getAngle()
{
	return (Real)dJointGetHingeAngle(_joint); 
}

Real HingeJoint::getAngleRate()
{
	return (Real)dJointGetHingeAngleRate(_joint); 
}

void HingeJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetHingeParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real HingeJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetHingeParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

HingeJoint::~HingeJoint()
{
}

SliderJoint::SliderJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateSlider(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void SliderJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetSliderAxis(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& SliderJoint::getAxis()
{
	dVector3 result;
	dJointGetSliderAxis(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

void SliderJoint::addForce(Real force,Real force2,Real force3)
{
	dJointAddSliderForce(_joint,(dReal)force);
}

Real SliderJoint::getPosition()
{
	return (Real)dJointGetSliderPosition(_joint); 
}

Real SliderJoint::getPositionRate()
{
	return (Real)dJointGetSliderPositionRate(_joint); 
}

void SliderJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetSliderParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real SliderJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetSliderParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

SliderJoint::~SliderJoint()
{
}

UniversalJoint::UniversalJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateUniversal(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void UniversalJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetUniversalAnchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}

void UniversalJoint::addTorque(Real torque,Real torque2,Real torque3)
{
	dJointAddUniversalTorques(_joint,(dReal)torque,(dReal)torque2);
}

const Ogre::Vector3& UniversalJoint::getAnchor()
{
	dVector3 result;
	dJointGetUniversalAnchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}

const Ogre::Vector3& UniversalJoint::getAnchorError()
{
	dVector3 result1,result2;
	dJointGetUniversalAnchor(_joint,result1); 
	dJointGetUniversalAnchor2(_joint,result2); 
	_anchor_error.x = (Real)(result1[0] - result2[0]);
	_anchor_error.y = (Real)(result1[1] - result2[1]);
	_anchor_error.z = (Real)(result1[2] - result2[2]);
	return _anchor_error;
}

void UniversalJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetUniversalAxis1(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& UniversalJoint::getAxis()
{
	dVector3 result;
	dJointGetUniversalAxis1(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

void UniversalJoint::setAdditionalAxis(const Ogre::Vector3& axis)
{
	dJointSetUniversalAxis2(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& UniversalJoint::getAdditionalAxis()
{
	dVector3 result;
	dJointGetUniversalAxis2(_joint,result); 
	_additional_axis.x = (Real)result[0];
	_additional_axis.y = (Real)result[1];
	_additional_axis.z = (Real)result[2];
	return _additional_axis;
}

void UniversalJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetUniversalParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real UniversalJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetUniversalParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

UniversalJoint::~UniversalJoint()
{
}

FixedJoint::FixedJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateFixed(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void FixedJoint::attach(const Body* body)
{
	Joint::attach(body);
	dJointSetFixed(_joint); 
}

void FixedJoint::attach(const Body* body_a,const Body* body_b)
{
	Joint::attach(body_a,body_b);
	dJointSetFixed(_joint); 
}

FixedJoint::~FixedJoint()
{
}

SuspensionJoint::SuspensionJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateHinge2(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void SuspensionJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetHinge2Anchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}


void SuspensionJoint::addTorque(Real torque,Real torque2,Real torque3)
{
	dJointAddHinge2Torques(_joint,(dReal)torque,(dReal)torque2);
}

const Ogre::Vector3& SuspensionJoint::getAnchor()
{
	dVector3 result;
	dJointGetHinge2Anchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}

const Ogre::Vector3& SuspensionJoint::getAdditionalAnchor()
{
	dVector3 result;
	dJointGetHinge2Anchor2(_joint,result);
	_anchor2.x = (Real)result[0];
	_anchor2.y = (Real)result[1];
	_anchor2.z = (Real)result[2];
	return _anchor2;
}

const Ogre::Vector3& SuspensionJoint::getAnchorError()
{
	dVector3 result1,result2;
	dJointGetHinge2Anchor(_joint,result1); 
	dJointGetHinge2Anchor2(_joint,result2); 
	_anchor_error.x = (Real)(result1[0] - result2[0]);
	_anchor_error.y = (Real)(result1[1] - result2[1]);
	_anchor_error.z = (Real)(result1[2] - result2[2]);
	return _anchor_error;
}

void SuspensionJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetHinge2Axis1(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& SuspensionJoint::getAxis()
{
	dVector3 result;
	dJointGetHinge2Axis1(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

void SuspensionJoint::setAdditionalAxis(const Ogre::Vector3& axis)
{
	dJointSetHinge2Axis2(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& SuspensionJoint::getAdditionalAxis()
{
	dVector3 result;
	dJointGetHinge2Axis2(_joint,result); 
	_additional_axis.x = (Real)result[0];
	_additional_axis.y = (Real)result[1];
	_additional_axis.z = (Real)result[2];
	return _additional_axis;
}

Real SuspensionJoint::getAngle()
{
	return (Real)dJointGetHinge2Angle1(_joint); 
}

Real SuspensionJoint::getAngleRate()
{
	return (Real)dJointGetHinge2Angle1Rate(_joint); 
}

Real SuspensionJoint::getPositionRate()
{
	return (Real)dJointGetHinge2Angle2Rate(_joint); 
}

void SuspensionJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetHinge2Param(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real SuspensionJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetHinge2Param(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

SuspensionJoint::~SuspensionJoint()
{
}

AngularMotorJoint::AngularMotorJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreateAMotor(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void AngularMotorJoint::setMode(AngularMotorJoint::Mode mode)
{
	dJointSetAMotorMode(_joint,(int)mode); 
}

AngularMotorJoint::Mode AngularMotorJoint::getMode()
{
	return (AngularMotorJoint::Mode)dJointGetAMotorMode(_joint); 
}

void AngularMotorJoint::addTorque(Real torque,Real torque2,Real torque3)
{
	dJointAddAMotorTorques(_joint,(dReal)torque,(dReal)torque2,(dReal)torque3);
}

void AngularMotorJoint::setAxisCount(int axes)
{
	assert((axes >= 0) && (axes <= 3));
	dJointSetAMotorNumAxes(_joint,axes); 
}

int AngularMotorJoint::getAxisCount()
{
	return dJointGetAMotorNumAxes(_joint); 
}

void AngularMotorJoint::setAxis(int axis_number,AngularMotorJoint::RelativeOrientation orientation,const Ogre::Vector3& axis)
{
	dJointSetAMotorAxis(_joint,axis_number,(int)orientation,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z); 
}

const Ogre::Vector3& AngularMotorJoint::getAxis(int axis_number)
{
	dVector3 result;
	dJointGetAMotorAxis(_joint,axis_number,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

AngularMotorJoint::RelativeOrientation AngularMotorJoint::getAxisRelativeOrientation(int axis_number)
{
	return (AngularMotorJoint::RelativeOrientation)dJointGetAMotorAxisRel(_joint,axis_number); 
}

void AngularMotorJoint::setAngle(int axis,Real angle)
{
	dJointSetAMotorAngle(_joint,axis,(dReal)angle); 
}

Real AngularMotorJoint::getAngle(int axis)
{
	return (Real)dJointGetAMotorAngle(_joint,axis); 
}

Real AngularMotorJoint::getAngleRate(int axis)
{
	return (Real)dJointGetAMotorAngleRate(_joint,axis); 
}

void AngularMotorJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetAMotorParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real AngularMotorJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetAMotorParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

AngularMotorJoint::~AngularMotorJoint()
{
}

PlanarJoint::PlanarJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreatePlane2D(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

void PlanarJoint::setParameterX(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetPlane2DXParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value);
}

void PlanarJoint::setParameterY(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetPlane2DYParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value);
}

void PlanarJoint::setParameterAngle(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetPlane2DAngleParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value);
}

PlanarJoint::~PlanarJoint()
{
}



SliderHingeJoint::SliderHingeJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreatePR(getWorldID(),getJointGroupID(group)); 
	registerJoint();
}

SliderHingeJoint::~SliderHingeJoint()
{
}

void SliderHingeJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetPRAnchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}

const Ogre::Vector3& SliderHingeJoint::getAnchor()
{
	dVector3 result;
	dJointGetPRAnchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}


Real SliderHingeJoint::getPosition()
{
	return (Real)dJointGetPRPosition(_joint); 
}

Real SliderHingeJoint::getPositionRate()
{
	return (Real)dJointGetPRPositionRate(_joint); 
}

void SliderHingeJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetPRAxis1(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& SliderHingeJoint::getAxis()
{
	dVector3 result;
	dJointGetPRAxis1(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

void SliderHingeJoint::setAdditionalAxis(const Ogre::Vector3& axis)
{
	dJointSetPRAxis2(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& SliderHingeJoint::getAdditionalAxis()
{
	dVector3 result;
	dJointGetPRAxis2(_joint,result); 
	_additional_axis.x = (Real)result[0];
	_additional_axis.y = (Real)result[1];
	_additional_axis.z = (Real)result[2];
	return _additional_axis;
}

void SliderHingeJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetPRParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value); 
}

Real SliderHingeJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetPRParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

void SliderHingeJoint::addTorque(Real torque,Real torque2,Real torque3)
{
	dJointAddPRTorque(_joint, (dReal)torque);
}



#if ODE_VERSION_MINOR > 9

PistonJoint::PistonJoint(World *world, const JointGroup* group) : Joint(world, group)
{
	_joint = dJointCreatePiston(getWorldID(), getJointGroupID(group)); 
	registerJoint();
}

void PistonJoint::setAnchor(const Ogre::Vector3& position)
{
	dJointSetPistonAnchor(_joint,(dReal)position.x,(dReal)position.y,(dReal)position.z); 
}

const Ogre::Vector3& PistonJoint::getAnchor()
{
	dVector3 result;
	dJointGetPistonAnchor(_joint,result); 
	_anchor.x = (Real)result[0];
	_anchor.y = (Real)result[1];
	_anchor.z = (Real)result[2];
	return _anchor;
}

const Ogre::Vector3& PistonJoint::getAnchorError()
{
	dVector3 result1,result2;
	dJointGetPistonAnchor(_joint,result1); 
	dJointGetPistonAnchor2(_joint,result2); 
	_anchor_error.x = (Real)(result1[0] - result2[0]);
	_anchor_error.y = (Real)(result1[1] - result2[1]);
	_anchor_error.z = (Real)(result1[2] - result2[2]);
	return _anchor_error;
}

void PistonJoint::setAxis(const Ogre::Vector3& axis)
{
	dJointSetPistonAxis(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z);
}

const Ogre::Vector3& PistonJoint::getAxis()
{
	dVector3 result;
	dJointGetPistonAxis(_joint,result); 
	_axis.x = (Real)result[0];
	_axis.y = (Real)result[1];
	_axis.z = (Real)result[2];
	return _axis;
}

void PistonJoint::setAxisDelta(const Ogre::Vector3& axis, const Ogre::Vector3& initalPosition)
{
	dJointSetPistonAxisDelta(_joint,(dReal)axis.x,(dReal)axis.y,(dReal)axis.z,
		(dReal)initalPosition.x,(dReal)initalPosition.y,(dReal)initalPosition.z);
}

Real PistonJoint::getPosition()
{
	return (Real)dJointGetPistonPosition(_joint); 
}

Real PistonJoint::getPositionRate()
{
	return (Real)dJointGetPistonPositionRate(_joint); 
}

Real PistonJoint::getAngle()
{
	return (Real)dJointGetPistonAngle(_joint); 
}

Real PistonJoint::getAngleRate()
{
	return (Real)dJointGetPistonAngleRate(_joint); 
}

void PistonJoint::addForce(Real force,Real force2,Real force3)
{
	dJointAddPistonForce(_joint,(dReal)force);
}

void PistonJoint::setParameter(Joint::Parameter parameter,Real value,int axis)
{
	dJointSetPistonParam(_joint,((int)parameter) + dParamGroup * (axis - 1),(dReal)value);  
}

Real PistonJoint::getParameter(Joint::Parameter parameter,int axis)
{
	return (Real)dJointGetPistonParam(_joint,((int)parameter) + dParamGroup * (axis - 1));
}

#endif /* ODE_VERSION_MINOR > 9 */