addons/ODE/source/IoODEBody.c

   1
   2 //metadoc ODEBody copyright Jonathan Wright, 2006
   3 //metadoc ODEBody license BSD revised
   4 /*metadoc ODEBody description
   5 ODEBody binding
   6 */
   7
   8 #include "IoODEBody.h"
   9 #include "IoODEMass.h"
  10 #include "IoState.h"
  11 #include "IoSeq.h"
  12 #include "IoVector_ode.h"
  13 #include "GLIncludes.h"
  14
  15 #define DATA(self) ((IoODEBodyData *)IoObject_dataPointer(self))
  16 #define BODYID (DATA(self)->bodyId)
  17 #define WORLD (DATA(self)->world)
  18
  19 void IoODEBody_assertValidBody(IoODEBody *self, IoObject *locals, IoMessage *m)
  20 {
  21         IOASSERT(WORLD, "This ODE body belongs to an ODE world which has been freed.");
  22         IOASSERT(BODYID, "ODE World Body cannot be used directly. Clone the world and use the Body on the cloned world.");
  23 }
  24
  25
  26 IoODEBody *IoMessage_locals_odeBodyArgAt_(IoMessage *self, void *locals, int n)
  27 {
  28         IoObject *b = IoMessage_locals_valueArgAt_(self, locals, n);
  29
  30         if (!ISODEBODY(b) && !ISNIL(b))
  31         {
  32                 IoMessage_locals_numberArgAt_errorForType_(self, locals, n, "ODEBody");
  33         }
  34
  35         return b;
  36 }
  37
  38 dBodyID IoMessage_locals_odeBodyIdArgAt_(IoMessage *self, void *locals, int n)
  39 {
  40         IoObject *body = IoMessage_locals_odeBodyArgAt_(self, locals, n);
  41         if (ISNIL(body))
  42         {
  43                 return 0;
  44         }
  45         else
  46         {
  47                 return DATA(body)->bodyId;
  48         }
  49 }
  50
  51
  52 IoTag *IoODEBody_newTag(void *state)
  53 {
  54         IoTag *tag = IoTag_newWithName_("ODEBody");
  55         IoTag_state_(tag, state);
  56         IoTag_freeFunc_(tag, (IoTagFreeFunc *)IoODEBody_free);
  57         IoTag_markFunc_(tag, (IoTagMarkFunc *)IoODEBody_mark);
  58         IoTag_cloneFunc_(tag, (IoTagCloneFunc *)IoODEBody_rawClone);
  59         return tag;
  60 }
  61
  62 IoODEBody *IoODEBody_proto(void *state)
  63 {
  64         IoObject *self = IoObject_new(state);
  65         IoObject_tag_(self, IoODEBody_newTag(state));
  66
  67         IoObject_setDataPointer_(self, calloc(1, sizeof(IoODEBodyData)));
  68
  69         BODYID = 0;
  70         WORLD = 0L;
  71
  72         IoState_registerProtoWithFunc_(state, self, IoODEBody_proto);
  73
  74         {
  75                 IoMethodTable methodTable[] = {
  76                 {"bodyId", IoODEBody_bodyId},
  77                 {"world", IoODEBody_world},
  78                 {"mass", IoODEBody_mass},
  79                 {"setMass", IoODEBody_setMass},
  80                 {"position", IoODEBody_position},
  81                 {"setPosition", IoODEBody_setPosition},
  82
  83                 {"force", IoODEBody_force},
  84                 {"setForce", IoODEBody_setForce},
  85                 {"addForce", IoODEBody_addForce},
  86                 {"addRelForce", IoODEBody_addRelForce},
  87
  88                 {"torque", IoODEBody_torque},
  89                 {"setTorque", IoODEBody_setTorque},
  90                 {"addTorque", IoODEBody_addTorque},
  91                 {"addRelTorque", IoODEBody_addRelTorque},
  92
  93                 {"linearVelocity", IoODEBody_linearVelocity},
  94                 {"setLinearVelocity", IoODEBody_setLinearVelocity},
  95
  96                 {"quaternion", IoODEBody_quaternion},
  97                 //{"setQuaternion", IoODEBody_setQuaternion},
  98                 {"rotation", IoODEBody_rotation},
  99                 {"setRotation", IoODEBody_setRotation},
 100                 {"glMultMatrix", IoODEBody_glMultMatrix},
 101
 102                 {NULL, NULL},
 103                 };
 104                 IoObject_addMethodTable_(self, methodTable);
 105         }
 106         return self;
 107 }
 108
 109 IoODEBody *IoODEBody_rawClone(IoODEBody *proto)
 110 {
 111         IoObject *self = IoObject_rawClonePrimitive(proto);
 112         IoObject_setDataPointer_(self, calloc(1, sizeof(IoODEBodyData)));
 113
 114         if(DATA(proto)->world)
 115         {
 116                 IoODEWorld *world = DATA(proto)->world;
 117
 118                 WORLD = world;
 119                 IoODEWorld_addBody(world, self);
 120                 BODYID = dBodyCreate(IoODEWorld_rawWorldId(world));
 121                                 dBodySetData(BODYID, self);
 122         }
 123         return self;
 124 }
 125
 126 void IoODEBody_free(IoODEBody *self)
 127 {
 128         if(BODYID && WORLD)
 129         {
 130                 IoODEWorld_removeBody(WORLD, self);
 131                 dBodyDestroy(BODYID);
 132         }
 133         free(IoObject_dataPointer(self));
 134 }
 135
 136 void IoODEBody_mark(IoODEBody *self)
 137 {
 138         if(WORLD)
 139         {
 140                 IoObject_shouldMark((IoObject *)WORLD);
 141         }
 142 }
 143
 144 IoODEBody *IoODEBody_new(void *state)
 145 {
 146         IoODEBody *proto = IoState_protoWithInitFunction_(state, IoODEBody_proto);
 147         return IOCLONE(proto);
 148 }
 149
 150 IoODEBody *IoODEBody_newBodyProtoWithWorld(void *state, IoODEWorld *world)
 151 {
 152         IoODEBody *proto = IoState_protoWithInitFunction_(state, IoODEBody_proto);
 153         IoODEBody *self = IOCLONE(proto);
 154         WORLD = world;
 155         return self;
 156 }
 157
 158 IoObject *IoODEBody_bodyFromId(void *state, dBodyID id)
 159 {
 160                 if (id == 0)
 161                 {
 162                                 return ((IoState*)state)->ioNil;
 163                 }
 164                 else
 165                 {
 166                                 return (IoODEBody*)dBodyGetData(id);
 167                 }
 168 }
 169
 170 /* ----------------------------------------------------------- */
 171
 172 void IoODEBody_worldDestroyed(IoODEBody *self)
 173 {
 174         WORLD = 0L;
 175         BODYID = 0;
 176 }
 177
 178 /* ----------------------------------------------------------- */
 179
 180
 181 IoObject *IoODEBody_bodyId(IoODEBody *self, IoObject *locals, IoMessage *m)
 182 {
 183         return IONUMBER((long)BODYID);
 184 }
 185
 186 IoObject *IoODEBody_world(IoODEBody *self, IoObject *locals, IoMessage *m)
 187 {
 188         return WORLD ? WORLD : IONIL(self);
 189 }
 190
 191 IoObject *IoODEBody_mass(IoODEBody *self, IoObject *locals, IoMessage *m)
 192 {
 193         IoODEBody_assertValidBody(self, locals, m);
 194
 195         {
 196                 IoODEMass *mass = IoODEMass_new(IOSTATE);
 197                 dBodyGetMass(BODYID, IoODEMass_dMassStruct(mass));
 198                 return mass;
 199         }
 200 }
 201
 202 IoObject *IoODEBody_setMass(IoODEBody *self, IoObject *locals, IoMessage *m)
 203 {
 204         IoODEBody_assertValidBody(self, locals, m);
 205
 206         {
 207                 dMass *mass = IoMessage_locals_odeMassStructArgAt_(m, locals, 0);
 208                 dBodySetMass(BODYID, mass);
 209         }
 210
 211         return self;
 212 }
 213
 214 IoObject *IoODEBody_position(IoODEBody *self, IoObject *locals, IoMessage *m)
 215 {
 216         IoODEBody_assertValidBody(self, locals, m);
 217         return IoVector_newWithODEPoint(IOSTATE, dBodyGetPosition(BODYID));
 218 }
 219
 220 IoObject *IoODEBody_setPosition(IoODEBody *self, IoObject *locals, IoMessage *m)
 221 {
 222         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 223         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 224         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 225
 226         IoODEBody_assertValidBody(self, locals, m);
 227         dBodySetPosition(BODYID, x, y, z);
 228         return self;
 229 }
 230
 231 IoObject *IoODEBody_force(IoODEBody *self, IoObject *locals, IoMessage *m)
 232 {
 233         IoODEBody_assertValidBody(self, locals, m);
 234         return IoVector_newWithODEPoint(IOSTATE, dBodyGetForce(BODYID));
 235 }
 236
 237 IoObject *IoODEBody_setForce(IoODEBody *self, IoObject *locals, IoMessage *m)
 238 {
 239         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 240         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 241         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 242
 243         IoODEBody_assertValidBody(self, locals, m);
 244         dBodySetForce(BODYID, x, y, z);
 245         return self;
 246 }
 247
 248 IoObject *IoODEBody_addForce(IoODEBody *self, IoObject *locals, IoMessage *m)
 249 {
 250         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 251         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 252         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 253
 254         IoODEBody_assertValidBody(self, locals, m);
 255         dBodyAddForce(BODYID, x, y, z);
 256         return self;
 257 }
 258
 259 IoObject *IoODEBody_addRelForce(IoODEBody *self, IoObject *locals, IoMessage *m)
 260 {
 261         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 262         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 263         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 264
 265         IoODEBody_assertValidBody(self, locals, m);
 266         dBodyAddRelForce(BODYID, x, y, z);
 267         return self;
 268 }
 269
 270 IoObject *IoODEBody_torque(IoODEBody *self, IoObject *locals, IoMessage *m)
 271 {
 272         IoODEBody_assertValidBody(self, locals, m);
 273         return IoVector_newWithODEPoint(IOSTATE, dBodyGetTorque(BODYID));
 274 }
 275
 276 IoObject *IoODEBody_setTorque(IoODEBody *self, IoObject *locals, IoMessage *m)
 277 {
 278         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 279         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 280         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 281
 282         IoODEBody_assertValidBody(self, locals, m);
 283         dBodySetTorque(BODYID, x, y, z);
 284         return self;
 285 }
 286
 287 IoObject *IoODEBody_addTorque(IoODEBody *self, IoObject *locals, IoMessage *m)
 288 {
 289         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 290         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 291         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 292
 293         IoODEBody_assertValidBody(self, locals, m);
 294         dBodyAddTorque(BODYID, x, y, z);
 295         return self;
 296 }
 297
 298 IoObject *IoODEBody_addRelTorque(IoODEBody *self, IoObject *locals, IoMessage *m)
 299 {
 300         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 301         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 302         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 303
 304         IoODEBody_assertValidBody(self, locals, m);
 305         dBodyAddRelTorque(BODYID, x, y, z);
 306         return self;
 307 }
 308
 309 IoObject *IoODEBody_linearVelocity(IoODEBody *self, IoObject *locals, IoMessage *m)
 310 {
 311         IoODEBody_assertValidBody(self, locals, m);
 312         return IoVector_newWithODEPoint(IOSTATE, dBodyGetLinearVel(BODYID));
 313 }
 314
 315 IoObject *IoODEBody_setLinearVelocity(IoODEBody *self, IoObject *locals, IoMessage *m)
 316 {
 317         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 318         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 319         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 320
 321         IoODEBody_assertValidBody(self, locals, m);
 322         dBodySetLinearVel(BODYID, x, y, z);
 323         return self;
 324 }
 325
 326 IoObject *IoODEBody_glMultMatrix(IoODEBody *self, IoObject *locals, IoMessage *m)
 327 {
 328         IoODEBody_assertValidBody(self, locals, m);
 329         {
 330                 dBodyID bodyId = BODYID;
 331
 332                 const dReal *pos = dBodyGetPosition(bodyId);
 333                 const dReal *R = dBodyGetRotation(bodyId);
 334
 335                 GLfloat matrix[16] = {R[0], R[4], R[8], 0, R[1], R[5], R[9], 0, R[2], R[6], R[10], 0, pos[0], pos[1], pos[2], 1};
 336                 glMultMatrixf(matrix);
 337                 return self;
 338         }
 339 }
 340
 341 IoObject *IoODEBody_rotation(IoODEBody *self, IoObject *locals, IoMessage *m)
 342 {
 343         IoODEBody_assertValidBody(self, locals, m);
 344         {
 345                 const dReal *R = dBodyGetRotation(BODYID);
 346
 347                 IoSeq *v = IoSeq_makeFloatArrayOfSize_(IOSTATE, 9);
 348                 UArray *u = IoSeq_rawUArray(v);
 349
 350                 UArray_at_put_(u, 0, R[0]);
 351                 UArray_at_put_(u, 1, R[4]);
 352                 UArray_at_put_(u, 2, R[8]);
 353
 354                 UArray_at_put_(u, 3, R[1]);
 355                 UArray_at_put_(u, 4, R[5]);
 356                 UArray_at_put_(u, 5, R[9]);
 357
 358                 UArray_at_put_(u, 6, R[2]);
 359                 UArray_at_put_(u, 7, R[6]);
 360                 UArray_at_put_(u, 8, R[10]);
 361
 362                 return v;
 363         }
 364 }
 365
 366 IoObject *IoODEBody_setRotation(IoODEBody *self, IoObject *locals, IoMessage *m)
 367 {
 368         IoODEBody_assertValidBody(self, locals, m);
 369         {
 370                 dMatrix3 R;
 371                 R[0] = IoMessage_locals_doubleArgAt_(m, locals, 0);
 372                 R[1] = IoMessage_locals_doubleArgAt_(m, locals, 1);
 373                 R[2] = IoMessage_locals_doubleArgAt_(m, locals, 2);
 374                 R[3] = 0;
 375                 R[4] = IoMessage_locals_doubleArgAt_(m, locals, 3);
 376                 R[5] = IoMessage_locals_doubleArgAt_(m, locals, 4);
 377                 R[6] = IoMessage_locals_doubleArgAt_(m, locals, 5);
 378                 R[7] = 0;
 379                 R[8] = IoMessage_locals_doubleArgAt_(m, locals, 6);
 380                 R[9] = IoMessage_locals_doubleArgAt_(m, locals, 7);
 381                 R[10] = IoMessage_locals_doubleArgAt_(m, locals, 8);
 382                 R[11] = 0;
 383
 384                 dBodySetRotation(BODYID, R);
 385                 return self;
 386         }
 387 }
 388
 389 IoObject *IoODEBody_quaternion(IoODEBody *self, IoObject *locals, IoMessage *m)
 390 {
 391         IoODEBody_assertValidBody(self, locals, m);
 392         {
 393                 const dReal *q = dBodyGetQuaternion(BODYID);
 394                 IoSeq *v = IoSeq_makeFloatArrayOfSize_(IOSTATE, 4);
 395                 UArray *u = IoSeq_rawUArray(v);
 396                 UArray_at_put_(u, 0, q[0]);
 397                 UArray_at_put_(u, 1, q[1]);
 398                 UArray_at_put_(u, 2, q[2]);
 399                 UArray_at_put_(u, 3, q[3]);
 400                 return v;
 401         }
 402 }
 403
 404 /*
 405 IoObject *IoODEBody_setQuaternion(IoODEBody *self, IoObject *locals, IoMessage *m)
 406 {
 407         const double w = IoMessage_locals_doubleArgAt_(m, locals, 0);
 408         const double x = IoMessage_locals_doubleArgAt_(m, locals, 1);
 409         const double y = IoMessage_locals_doubleArgAt_(m, locals, 2);
 410         const double z = IoMessage_locals_doubleArgAt_(m, locals, 3);
 411
 412         IoODEBody_assertValidBody(self, locals, m);
 413         dBodySetQuaternion(BODYID, w, x, y, z);
 414         return self;
 415 }
 416 */