addons/ODE/source/IoODEMass.c

   1
   2 //metadoc ODEMass copyright Jonathan Wright, 2006
   3 //metadoc ODEMass license BSD revised
   4 /*metadoc ODEMass description
   5 ODEMass binding
   6 */
   7
   8 #include "IoODEMass.h"
   9 #include "IoState.h"
  10 #include "IoSeq.h"
  11
  12 #define DATA(self) ((IoODEMassData *)IoObject_dataPointer(self))
  13
  14 IoTag *IoODEMass_newTag(void *state)
  15 {
  16         IoTag *tag = IoTag_newWithName_("ODEMass");
  17         IoTag_state_(tag, state);
  18         IoTag_freeFunc_(tag, (IoTagFreeFunc *)IoODEMass_free);
  19         IoTag_markFunc_(tag, (IoTagMarkFunc *)IoODEMass_mark);
  20         IoTag_cloneFunc_(tag, (IoTagCloneFunc *)IoODEMass_rawClone);
  21         return tag;
  22 }
  23
  24 IoODEMass *IoODEMass_proto(void *state)
  25 {
  26         IoObject *self = IoObject_new(state);
  27         IoObject_tag_(self, IoODEMass_newTag(state));
  28
  29         IoObject_setDataPointer_(self, calloc(1, sizeof(IoODEMassData)));
  30
  31         IoState_registerProtoWithFunc_(state, self, IoODEMass_proto);
  32
  33         {
  34                 IoMethodTable methodTable[] = {
  35                 {"reset", IoODEMass_reset},
  36                 {"mass", IoODEMass_mass},
  37                 {"setMass", IoODEMass_setMass},
  38                 {"centerOfGravity", IoODEMass_centerOfGravity},
  39                 {"setCenterOfGravity", IoODEMass_setCenterOfGravity},
  40                 {"inertiaTensor", IoODEMass_inertiaTensor},
  41                 {"parameters", IoODEMass_parameters},
  42                 {"setParameters", IoODEMass_setParameters},
  43
  44                 {"setSphereDensity", IoODEMass_setSphereDensity},
  45                 {"setSphereMass", IoODEMass_setSphereMass},
  46                 {"setCappedCylinderDensity", IoODEMass_setCappedCylinderDensity},
  47                 {"setCappedCylinderMass", IoODEMass_setCappedCylinderMass},
  48                 {"setBoxDensity", IoODEMass_setBoxDensity},
  49                 {"setBoxMass", IoODEMass_setBoxMass},
  50
  51                 {NULL, NULL},
  52                 };
  53                 IoObject_addMethodTable_(self, methodTable);
  54         }
  55         return self;
  56 }
  57
  58 IoODEMass *IoODEMass_rawClone(IoODEMass *proto)
  59 {
  60         IoObject *self = IoObject_rawClonePrimitive(proto);
  61         IoObject_setDataPointer_(self, calloc(1, sizeof(IoODEMassData)));
  62         memcpy(DATA(self), DATA(proto), sizeof(IoODEMassData));
  63         return self;
  64 }
  65
  66 void IoODEMass_free(IoODEMass *self)
  67 {
  68         free(IoObject_dataPointer(self));
  69 }
  70
  71 void IoODEMass_mark(IoODEMass *self)
  72 {
  73 }
  74
  75 IoODEMass *IoODEMass_new(void *state)
  76 {
  77         IoODEMass *proto = IoState_protoWithInitFunction_(state, IoODEMass_proto);
  78         return IOCLONE(proto);
  79 }
  80
  81
  82 /* ----------------------------------------------------------- */
  83
  84 IoODEMass *IoMessage_locals_odeMassArgAt_(IoMessage *self, void *locals, int n)
  85 {
  86         IoObject *m = IoMessage_locals_valueArgAt_(self, locals, n);
  87
  88         if (!ISODEMASS(m))
  89         {
  90                 IoMessage_locals_numberArgAt_errorForType_(self, locals, n, "ODEMass");
  91         }
  92
  93         return m;
  94 }
  95
  96 dMass *IoMessage_locals_odeMassStructArgAt_(IoMessage *self, void *locals, int n)
  97 {
  98         return IoODEMass_dMassStruct(IoMessage_locals_odeMassArgAt_(self, locals, n));
  99 }
 100
 101 dMass *IoODEMass_dMassStruct(IoODEMass *self)
 102 {
 103         return DATA(self);
 104 }
 105
 106 /* ----------------------------------------------------------- */
 107
 108 IoObject *IoODEMass_reset(IoODEMass *self, IoObject *locals, IoMessage *m)
 109 {
 110         dMassSetZero(DATA(self));
 111         return self;
 112 }
 113
 114 IoObject *IoODEMass_mass(IoODEMass *self, IoObject *locals, IoMessage *m)
 115 {
 116         return IONUMBER(DATA(self)->mass);
 117 }
 118
 119 IoObject *IoODEMass_setMass(IoODEMass *self, IoObject *locals, IoMessage *m)
 120 {
 121         //DATA(self)->mass = IoMessage_locals_doubleArgAt_(m, locals, 0);
 122         dMassAdjust(DATA(self), IoMessage_locals_doubleArgAt_(m, locals, 0));
 123         return self;
 124 }
 125
 126 IoObject *IoODEMass_centerOfGravity(IoODEMass *self, IoObject *locals, IoMessage *m)
 127 {
 128         vec3f v;
 129
 130         v.x = DATA(self)->c[0];
 131         v.y = DATA(self)->c[1];
 132         v.z = DATA(self)->c[2];
 133
 134         return IoSeq_newVec3f(IOSTATE, vector);
 135 }
 136
 137 IoObject *IoODEMass_setCenterOfGravity(IoODEMass *self, IoObject *locals, IoMessage *m)
 138 {
 139         const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
 140         const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
 141         const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
 142
 143         DATA(self)->c[0] = x;
 144         DATA(self)->c[1] = y;
 145         DATA(self)->c[2] = z;
 146         return self;
 147 }
 148
 149 IoObject *IoODEMass_inertiaTensor(IoODEMass *self, IoObject *locals, IoMessage *m)
 150 {
 151         UArray *u = UArray_new();
 152         int i, j;
 153
 154         UArray_setItemType_(u, CTYPE_float32_t);
 155         UArray_setSize_(vector, 9);
 156
 157         // I == vector(I11, I12, I13, _, I12, I22, I23, _, I13, I23, I33, _)
 158
 159
 160         for(i = 0, j = 0; i < 12; i++)
 161         {
 162                 if ((i + 1) % 4)
 163                 {
 164                         UArray_at_putDouble_(vector, j++, DATA(self)->I[i]);
 165                 }
 166         }
 167
 168         return IoSeq_newWithUArray_(IOSTATE, u);
 169 }
 170
 171 IoObject *IoODEMass_parameters(IoODEMass *self, IoObject *locals, IoMessage *m)
 172 {
 173         // vector(theMass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23)
 174
 175         UArray *u = UArray_new();
 176         int i, j = 0;
 177
 178         UArray_setItemType_(u, CTYPE_float32_t);
 179         UArray_setSize_(u, 10);
 180
 181         UArray_at_putDouble_(vector, j++, DATA(self)->mass);
 182
 183         for(i=0; i < 3; i++)
 184         {
 185                 UArray_at_putDouble_(vector, j++, DATA(self)->c[i]);
 186         }
 187
 188         //              0    1    2   3   4    5    6   7   8    9   10  11
 189         // I == vector(I11, I12, I13, _, I12, I22, I23, _, I13, I23, I33, _)
 190         UArray_at_putDouble_(vector, j++, DATA(self)->I[0 ]); // I11
 191         UArray_at_putDouble_(vector, j++, DATA(self)->I[5 ]); // I22
 192         UArray_at_putDouble_(vector, j++, DATA(self)->I[10]); // I33
 193         UArray_at_putDouble_(vector, j++, DATA(self)->I[1 ]); // I12
 194         UArray_at_putDouble_(vector, j++, DATA(self)->I[2 ]); // I13
 195         UArray_at_putDouble_(vector, j++, DATA(self)->I[6 ]); // I23
 196
 197         return IoSeq_newWithUArray_(IOSTATE, u);
 198 }
 199
 200 IoObject *IoODEMass_setParameters(IoODEMass *self, IoObject *locals, IoMessage *m)
 201 {
 202         const double theMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
 203
 204         const double cgx     = IoMessage_locals_doubleArgAt_(m, locals, 1);
 205         const double cgy     = IoMessage_locals_doubleArgAt_(m, locals, 2);
 206         const double cgz     = IoMessage_locals_doubleArgAt_(m, locals, 3);
 207
 208         const double I11     = IoMessage_locals_doubleArgAt_(m, locals, 4);
 209         const double I22     = IoMessage_locals_doubleArgAt_(m, locals, 5);
 210         const double I33     = IoMessage_locals_doubleArgAt_(m, locals, 6);
 211         const double I12     = IoMessage_locals_doubleArgAt_(m, locals, 7);
 212         const double I13     = IoMessage_locals_doubleArgAt_(m, locals, 8);
 213         const double I23     = IoMessage_locals_doubleArgAt_(m, locals, 9);
 214
 215         dMassSetParameters(DATA(self), theMass, cgx, cgy, cgz, I11, I22, I33, I12, I13, I23);
 216         return self;
 217 }
 218
 219 IoObject *IoODEMass_setSphereDensity(IoODEMass *self, IoObject *locals, IoMessage *m)
 220 {
 221         const double density = IoMessage_locals_doubleArgAt_(m, locals, 0);
 222         const double radius = IoMessage_locals_doubleArgAt_(m, locals, 1);
 223
 224         dMassSetSphere(DATA(self), density, radius);
 225         return self;
 226 }
 227
 228 IoObject *IoODEMass_setSphereMass(IoODEMass *self, IoObject *locals, IoMessage *m)
 229 {
 230         const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
 231         const double radius = IoMessage_locals_doubleArgAt_(m, locals, 1);
 232
 233         dMassSetSphereTotal(DATA(self), totalMass, radius);
 234         return self;
 235 }
 236
 237
 238 IoObject *IoODEMass_setCappedCylinderDensity(IoODEMass *self, IoObject *locals, IoMessage *m)
 239 {
 240         const double density   = IoMessage_locals_doubleArgAt_(m, locals, 0);
 241         const double direction = IoMessage_locals_doubleArgAt_(m, locals, 1);
 242         const double radius    = IoMessage_locals_doubleArgAt_(m, locals, 2);
 243         const double length    = IoMessage_locals_doubleArgAt_(m, locals, 3);
 244
 245         dMassSetCappedCylinder(DATA(self), density, direction, radius, length);
 246         return self;
 247 }
 248
 249 IoObject *IoODEMass_setCappedCylinderMass(IoODEMass *self, IoObject *locals, IoMessage *m)
 250 {
 251         const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
 252         const double direction = IoMessage_locals_doubleArgAt_(m, locals, 1);
 253         const double radius    = IoMessage_locals_doubleArgAt_(m, locals, 2);
 254         const double length    = IoMessage_locals_doubleArgAt_(m, locals, 3);
 255
 256         dMassSetCappedCylinderTotal(DATA(self), totalMass, direction, radius, length);
 257         return self;
 258 }
 259
 260 IoObject *IoODEMass_setBoxDensity(IoODEMass *self, IoObject *locals, IoMessage *m)
 261 {
 262         const double density = IoMessage_locals_doubleArgAt_(m, locals, 0);
 263         const double lx      = IoMessage_locals_doubleArgAt_(m, locals, 1);
 264         const double ly      = IoMessage_locals_doubleArgAt_(m, locals, 2);
 265         const double lz      = IoMessage_locals_doubleArgAt_(m, locals, 3);
 266
 267         dMassSetBox(DATA(self), density, lx, ly, lz);
 268         return self;
 269 }
 270
 271 IoObject *IoODEMass_setBoxMass(IoODEMass *self, IoObject *locals, IoMessage *m)
 272 {
 273         const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
 274         const double lx        = IoMessage_locals_doubleArgAt_(m, locals, 1);
 275         const double ly        = IoMessage_locals_doubleArgAt_(m, locals, 2);
 276         const double lz        = IoMessage_locals_doubleArgAt_(m, locals, 3);
 277
 278         dMassSetBoxTotal(DATA(self), totalMass, lx, ly, lz);
 279         return self;
 280 }