00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "body.h"
00022 #include "world.h"
00023 #include "../sceneserver/scene.h"
00024 #include "zeitgeist/logserver/logserver.h"
00025
00026 using namespace boost;
00027 using namespace oxygen;
00028 using namespace salt;
00029 using namespace std;
00030
00031 Body::Body() : ODEObject(), mODEBody(0)
00032 {
00033 }
00034
00035 Body::~Body()
00036 {
00037 if (mODEBody)
00038 {
00039 dBodyDestroy(mODEBody);
00040 mODEBody = 0;
00041 }
00042 }
00043
00044 dBodyID Body::GetODEBody() const
00045 {
00046 return mODEBody;
00047 }
00048
00049 void Body::Enable()
00050 {
00051 dBodyEnable(mODEBody);
00052 }
00053
00054 void Body::Disable()
00055 {
00056 dBodyDisable(mODEBody);
00057 }
00058
00059 bool Body::IsEnabled() const
00060 {
00061 return (dBodyIsEnabled(mODEBody) != 0);
00062 }
00063
00064 void Body::UseGravity(bool f)
00065 {
00066 if (f == true)
00067 {
00068
00069 dBodySetGravityMode(mODEBody, 1);
00070 }
00071 else
00072 {
00073
00074 dBodySetGravityMode(mODEBody, 0);
00075 }
00076 }
00077
00078 bool Body::UsesGravity() const
00079 {
00080 return (dBodyGetGravityMode(mODEBody) != 0);
00081 }
00082
00083 bool Body::CreateBody()
00084 {
00085 if (mODEBody != 0)
00086 {
00087 return true;
00088 }
00089
00090 dWorldID world = GetWorldID();
00091 if (world == 0)
00092 {
00093 return false;
00094 }
00095
00096
00097 mODEBody = dBodyCreate(world);
00098 if (mODEBody == 0)
00099 {
00100 GetLog()->Error()
00101 << "(Body) ERROR: could not create new ODE body\n";
00102 return false;
00103 }
00104
00105 return true;
00106 }
00107
00108 void Body::OnLink()
00109 {
00110 ODEObject::OnLink();
00111
00112 if (! CreateBody())
00113 {
00114 return;
00115 }
00116
00117
00118 dBodySetData(mODEBody, this);
00119
00120 shared_ptr<BaseNode> baseNode = shared_static_cast<BaseNode>
00121 (make_shared(GetParent()));
00122
00123 const Matrix& mat = baseNode->GetWorldTransform();
00124 SetRotation(mat);
00125 SetPosition(mat.Pos());
00126 }
00127
00128 void
00129 Body::SetMass(float mass)
00130 {
00131 dMass ODEMass;
00132 dBodyGetMass(mODEBody, &ODEMass);
00133 dMassAdjust(&ODEMass, mass);
00134 dBodySetMass(mODEBody, &ODEMass);
00135 }
00136
00137 float
00138 Body::GetMass() const
00139 {
00140 dMass m;
00141 dBodyGetMass(mODEBody, &m);
00142 return m.mass;
00143 }
00144
00145 void Body::GetMassParameters(dMass& mass) const
00146 {
00147 dBodyGetMass(mODEBody, &mass);
00148 }
00149
00150 void Body::SetMassParameters(const dMass& mass)
00151 {
00152 dBodySetMass(mODEBody, &mass);
00153 }
00154
00155 void Body::SetSphere(float density, float radius)
00156 {
00157 dMass ODEMass;
00158 dMassSetSphere(&ODEMass, density, radius);
00159 dBodySetMass(mODEBody, &ODEMass);
00160 }
00161
00162 void Body::SetSphereTotal(float total_mass, float radius)
00163 {
00164 dMass ODEMass;
00165 dMassSetSphereTotal(&ODEMass, total_mass, radius);
00166 dBodySetMass(mODEBody, &ODEMass);
00167 }
00168
00169 void Body::SetBox(float density, const Vector3f& size)
00170 {
00171 dMass ODEMass;
00172 dMassSetBox(&ODEMass, density, size[0], size[1], size[2]);
00173 dBodySetMass(mODEBody, &ODEMass);
00174 }
00175
00176 void Body::SetBoxTotal(float total_mass, const salt::Vector3f& size)
00177 {
00178 dMass ODEMass;
00179 dMassSetBoxTotal(&ODEMass, total_mass, size[0], size[1], size[2]);
00180 dBodySetMass(mODEBody, &ODEMass);
00181 }
00182
00183 void Body::SetCylinder (float density, float radius, float length)
00184 {
00185 dMass ODEMass;
00186
00187
00188 int direction = 3;
00189
00190 dMassSetCylinder (&ODEMass, density, direction, radius, length);
00191 dBodySetMass(mODEBody, &ODEMass);
00192 }
00193
00194 void Body::SetCylinderTotal(float total_mass, float radius, float length)
00195 {
00196 dMass ODEMass;
00197
00198
00199 int direction = 3;
00200
00201 dMassSetCylinderTotal(&ODEMass, total_mass, direction, radius, length);
00202 dBodySetMass(mODEBody, &ODEMass);
00203 }
00204
00205 void Body::SetCappedCylinder (float density, float radius, float length)
00206 {
00207 dMass ODEMass;
00208
00209
00210 int direction = 3;
00211
00212 dMassSetCappedCylinder (&ODEMass, density, direction, radius, length);
00213 dBodySetMass(mODEBody, &ODEMass);
00214 }
00215
00216 void Body::SetCappedCylinderTotal(float total_mass, float radius, float length)
00217 {
00218 dMass ODEMass;
00219
00220
00221 int direction = 3;
00222
00223 dMassSetCappedCylinderTotal(&ODEMass, total_mass,
00224 direction, radius, length);
00225 dBodySetMass(mODEBody, &ODEMass);
00226 }
00227
00228 Vector3f Body::GetVelocity() const
00229 {
00230 const dReal* vel = dBodyGetLinearVel(mODEBody);
00231 return Vector3f(vel[0], vel[1], vel[2]);
00232 }
00233
00234 void Body::SetVelocity(const Vector3f& vel)
00235 {
00236 dBodySetLinearVel(mODEBody, vel[0], vel[1], vel[2]);
00237 }
00238
00239 void
00240 Body::SetRotation(const Matrix& rot)
00241 {
00242 dMatrix3 m;
00243 ConvertRotationMatrix(rot,m);
00244 dBodySetRotation(mODEBody,m);
00245 }
00246
00247 Vector3f
00248 Body::GetAngularVelocity() const
00249 {
00250 const dReal* vel = dBodyGetAngularVel(mODEBody);
00251 return Vector3f(vel[0], vel[1], vel[2]);
00252 }
00253
00254 void
00255 Body::SetAngularVelocity(const Vector3f& vel)
00256 {
00257 dBodySetAngularVel(mODEBody, vel[0], vel[1], vel[2]);
00258 }
00259
00260 void Body::PostPhysicsUpdateInternal()
00261 {
00262
00263
00264 const dReal* pos = dBodyGetPosition(mODEBody);
00265 const dReal* rot = dBodyGetRotation(mODEBody);
00266
00267 shared_ptr<BaseNode> baseNode = shared_static_cast<BaseNode>
00268 (make_shared(GetParent()));
00269
00270 Matrix mat;
00271 mat.m[0] = rot[0];
00272 mat.m[1] = rot[4];
00273 mat.m[2] = rot[8];
00274 mat.m[3] = 0;
00275 mat.m[4] = rot[1];
00276 mat.m[5] = rot[5];
00277 mat.m[6] = rot[9];
00278 mat.m[7] = 0;
00279 mat.m[8] = rot[2];
00280 mat.m[9] = rot[6];
00281 mat.m[10] = rot[10];
00282 mat.m[11] = 0;
00283 mat.m[12] = pos[0];
00284 mat.m[13] = pos[1];
00285 mat.m[14] = pos[2];
00286 mat.m[15] = 1;
00287
00288 baseNode->SetWorldTransform(mat);
00289 }
00290
00291 shared_ptr<Body> Body::GetBody(dBodyID id)
00292 {
00293 if (id == 0)
00294 {
00295 return shared_ptr<Body>();
00296 }
00297
00298 Body* bodyPtr =
00299 static_cast<Body*>(dBodyGetData(id));
00300
00301 if (bodyPtr == 0)
00302 {
00303
00304 cerr << "ERROR: (Body) no body found for dBodyID "
00305 << id << "\n";
00306 return shared_ptr<Body>();
00307 }
00308
00309 shared_ptr<Body> body = shared_static_cast<Body>
00310 (make_shared(bodyPtr->GetSelf()));
00311
00312 if (body.get() == 0)
00313 {
00314
00315 cerr << "ERROR: (Body) got no shared_ptr for dBodyID "
00316 << id << "\n";
00317 }
00318
00319 return body;
00320 }
00321
00322 void
00323 Body::AddForce(const Vector3f& force)
00324 {
00325 dBodyAddForce(mODEBody, force.x(), force.y(), force.z());
00326 }
00327
00328 void
00329 Body::AddTorque(const Vector3f& torque)
00330 {
00331 dBodyAddTorque(mODEBody, torque.x(), torque.y(), torque.z());
00332 }
00333
00334 void
00335 Body::SetPosition(const Vector3f& pos)
00336 {
00337 dBodySetPosition(mODEBody, pos.x(), pos.y(), pos.z());
00338
00339 }
00340
00341 Vector3f
00342 Body::GetPosition() const
00343 {
00344 const dReal* pos = dBodyGetPosition(mODEBody);
00345 return Vector3f(pos[0], pos[1], pos[2]);
00346 }