tests/joints/fixed.cpp

   1 /*************************************************************************
   2   *                                                                       *
   3   * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4   * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5   *                                                                       *
   6   * This library is free software; you can redistribute it and/or         *
   7   * modify it under the terms of EITHER:                                  *
   8   *   (1) The GNU Lesser General Public License as published by the Free  *
   9   *       Software Foundation; either version 2.1 of the License, or (at  *
  10   *       your option) any later version. The text of the GNU Lesser      *
  11   *       General Public License is included with this library in the     *
  12   *       file LICENSE.TXT.                                               *
  13   *   (2) The BSD-style license that is included with this library in     *
  14   *       the file LICENSE-BSD.TXT.                                       *
  15   *                                                                       *
  16   * This library is distributed in the hope that it will be useful,       *
  17   * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19   * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20   *                                                                       *
  21   *************************************************************************/
  22 //234567890123456789012345678901234567890123456789012345678901234567890123456789
  23 //        1         2         3         4         5         6         7
  24
  25 ////////////////////////////////////////////////////////////////////////////////
  26 // This file creates unit tests for some of the functions found in:
  27 // ode/src/joinst/fixed.cpp
  28 //
  29 //
  30 ////////////////////////////////////////////////////////////////////////////////
  31
  32 #include <UnitTest++.h>
  33 #include <ode/ode.h>
  34
  35 #include "../../ode/src/config.h"
  36 #include "../../ode/src/joints/fixed.h"
  37
  38 SUITE (TestdxJointFixed)
  39 {
  40     struct dxJointFixed_Fixture_1
  41     {
  42         dxJointFixed_Fixture_1()
  43         {
  44             wId = dWorldCreate();
  45
  46             bId1 = dBodyCreate (wId);
  47             dBodySetPosition (bId1, 0, -1, 0);
  48
  49             bId2 = dBodyCreate (wId);
  50             dBodySetPosition (bId2, 0, 1, 0);
  51
  52             jId   = dJointCreateFixed (wId, 0);
  53             joint = (dxJointFixed*) jId;
  54
  55
  56             dJointAttach (jId, bId1, bId2);
  57         }
  58
  59         ~dxJointFixed_Fixture_1()
  60         {
  61             dWorldDestroy (wId);
  62         }
  63
  64         dWorldID wId;
  65
  66         dBodyID bId1;
  67         dBodyID bId2;
  68
  69
  70         dJointID jId;
  71         dxJointFixed* joint;
  72     };
  73
  74     TEST_FIXTURE (dxJointFixed_Fixture_1, test_dJointSetFixed)
  75     {
  76         // the 2 bodies are align
  77         dJointSetFixed (jId);
  78         CHECK_CLOSE (joint->qrel[0], 1.0, 1e-4);
  79         CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4);
  80         CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4);
  81         CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4);
  82
  83         dMatrix3 R;
  84         // Rotate 2nd body 90deg around X
  85         dBodySetPosition (bId2, 0, 0, 1);
  86         dRFromAxisAndAngle (R, 1, 0, 0, M_PI/2.0);
  87         dBodySetRotation (bId2, R);
  88
  89         dJointSetFixed (jId);
  90         CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4);
  91         CHECK_CLOSE (joint->qrel[1], 0.70710678118654757, 1e-4);
  92         CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4);
  93         CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4);
  94
  95
  96         // Rotate 2nd body -90deg around X
  97         dBodySetPosition (bId2, 0, 0, -1);
  98         dRFromAxisAndAngle (R, 1, 0, 0, -M_PI/2.0);
  99         dBodySetRotation (bId2, R);
 100
 101         dJointSetFixed (jId);
 102         CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4);
 103         CHECK_CLOSE (joint->qrel[1], -0.70710678118654757, 1e-4);
 104         CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4);
 105         CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4);
 106
 107
 108         // Rotate 2nd body 90deg around Z
 109         dBodySetPosition (bId2, 0, 1, 0);
 110         dRFromAxisAndAngle (R, 0, 0, 1, M_PI/2.0);
 111         dBodySetRotation (bId2, R);
 112
 113         dJointSetFixed (jId);
 114         CHECK_CLOSE (joint->qrel[0], 0.70710678118654757, 1e-4);
 115         CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4);
 116         CHECK_CLOSE (joint->qrel[2], 0.0, 1e-4);
 117         CHECK_CLOSE (joint->qrel[3], 0.70710678118654757, 1e-4);
 118
 119
 120         // Rotate 2nd body 45deg around Y
 121         dBodySetPosition (bId2, 0, 1, 0);
 122         dRFromAxisAndAngle (R, 0, 1, 0, M_PI/4.0);
 123         dBodySetRotation (bId2, R);
 124
 125         dJointSetFixed (jId);
 126         CHECK_CLOSE (joint->qrel[0], 0.92387953251128674, 1e-4);
 127         CHECK_CLOSE (joint->qrel[1], 0.0, 1e-4);
 128         CHECK_CLOSE (joint->qrel[2], 0.38268343236508984, 1e-4);
 129         CHECK_CLOSE (joint->qrel[3], 0.0, 1e-4);
 130
 131         // Rotate in a strange manner
 132         // Both bodies at origin
 133         dRFromEulerAngles (R, REAL(0.23), REAL(3.1), REAL(-0.73));
 134         dBodySetPosition (bId1, 0, 0, 0);
 135         dBodySetRotation (bId1, R);
 136
 137         dRFromEulerAngles (R, REAL(-0.57), REAL(1.49), REAL(0.81));
 138         dBodySetPosition (bId2, 0, 0, 0);
 139         dBodySetRotation (bId2, R);
 140
 141         dJointSetFixed (jId);
 142         CHECK_CLOSE (joint->qrel[0], -0.25526036263124319, 1e-4);
 143         CHECK_CLOSE (joint->qrel[1],  0.28434861188441968, 1e-4);
 144         CHECK_CLOSE (joint->qrel[2], -0.65308047160141625, 1e-4);
 145         CHECK_CLOSE (joint->qrel[3],  0.65381489108282143, 1e-4);
 146     }
 147
 148
 149 } // End of SUITE TestdxJointFixed