00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "universaljoint.h"
00021 #include <zeitgeist/logserver/logserver.h>
00022
00023 using namespace oxygen;
00024 using namespace boost;
00025 using namespace salt;
00026
00027 UniversalJoint::UniversalJoint() : Joint()
00028 {
00029 }
00030
00031 UniversalJoint::~UniversalJoint()
00032 {
00033 }
00034
00035 void UniversalJoint::OnLink()
00036 {
00037 dWorldID world = GetWorldID();
00038
00039 if (world == 0)
00040 {
00041 return;
00042 }
00043
00044 mODEJoint = dJointCreateUniversal(world, 0);
00045 }
00046
00047 void UniversalJoint::SetAnchor(const Vector3f& anchor)
00048 {
00049
00050 Vector3f gAnchor(GetWorldTransform() * anchor);
00051 dJointSetUniversalAnchor (mODEJoint, gAnchor[0], gAnchor[1], gAnchor[2]);
00052 }
00053
00054 Vector3f UniversalJoint::GetAnchor(EBodyIndex idx)
00055 {
00056 Vector3f pos(0,0,0);
00057
00058 switch (idx)
00059 {
00060 case BI_FIRST:
00061 {
00062 dReal anchor[3];
00063 dJointGetUniversalAnchor (mODEJoint, anchor);
00064 pos = Vector3f(anchor[0],anchor[1],anchor[2]);
00065 }
00066
00067 case BI_SECOND:
00068 {
00069 dReal anchor[3];
00070 dJointGetUniversalAnchor2(mODEJoint, anchor);
00071 pos = Vector3f(anchor[0],anchor[1],anchor[2]);
00072 }
00073
00074 default:
00075 break;
00076 }
00077
00078 return GetLocalPos(pos);
00079 }
00080
00081 void UniversalJoint::SetAxis1(Vector3f & axis)
00082 {
00083 Vector3f first(GetWorldTransform().Rotate(axis));
00084 dJointSetUniversalAxis1(mODEJoint,first[0],first[1],first[2]);
00085 }
00086
00087 void UniversalJoint::SetAxis2(Vector3f & axis)
00088 {
00089 Vector3f second(GetWorldTransform().Rotate(axis));
00090 dJointSetUniversalAxis2(mODEJoint,second[0],second[1],second[2]);
00091 }
00092
00093 Vector3f UniversalJoint::GetAxis(EAxisIndex idx)
00094 {
00095 Vector3f vec(0,0,0);
00096
00097 switch (idx)
00098 {
00099 case AI_FIRST:
00100 {
00101 dReal axis[3];
00102 dJointGetUniversalAxis1(mODEJoint, axis);
00103 vec = Vector3f(axis[0],axis[1],axis[2]);
00104 }
00105
00106 case AI_SECOND:
00107 {
00108 dReal axis[3];
00109 dJointGetUniversalAxis2(mODEJoint, axis);
00110 vec = Vector3f(axis[0],axis[1],axis[2]);
00111 }
00112
00113 default:
00114 break;
00115 }
00116
00117 return GetLocalPos(vec);
00118 }
00119
00120 float UniversalJoint::GetAngle(EAxisIndex idx)
00121 {
00122 switch (idx)
00123 {
00124 case AI_FIRST:
00125 return gRadToDeg(dJointGetUniversalAngle1(mODEJoint));
00126
00127 case AI_SECOND:
00128 return gRadToDeg(dJointGetUniversalAngle2(mODEJoint));
00129
00130 default:
00131 return 0;
00132 }
00133 }
00134
00135 float UniversalJoint::GetAngleRate(EAxisIndex idx)
00136 {
00137 switch (idx)
00138 {
00139 case AI_FIRST:
00140 return gRadToDeg(dJointGetUniversalAngle1Rate(mODEJoint));
00141
00142 case AI_SECOND:
00143 return gRadToDeg(dJointGetUniversalAngle2Rate(mODEJoint));
00144
00145 default:
00146 return 0;
00147 }
00148 }
00149
00150 void UniversalJoint::SetParameter(int parameter, float value)
00151 {
00152 dJointSetUniversalParam(mODEJoint, parameter, value);
00153 }
00154
00155 float UniversalJoint::GetParameter(int parameter)
00156 {
00157 return dJointGetUniversalParam(mODEJoint, parameter);
00158 }
00159
00160
00161
00162
00163
00164
00165
00166