tests/odemath.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 #include <UnitTest++.h>
  26 #include <ode/ode.h>
  27 #include <ode/odemath.h>
  28
  29
  30
  31 TEST(test_dNormalization3)
  32 {
  33     const dVector3 x = {1,0,0,0};
  34     const dVector3 y = {0,1,0,0};
  35     const dVector3 z = {0,0,1,0};
  36     dVector3 v;
  37
  38     // Check when value in first component (i.e. [0])
  39     v[0] = REAL(1.0);
  40     v[1] = REAL(0.0);
  41     v[2] = REAL(0.0);
  42     dSafeNormalize3(v);
  43     CHECK_ARRAY_CLOSE(x, v, 3, 1e-6);
  44     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  45
  46     v[0] = REAL(0.1);
  47     v[1] = REAL(0.0);
  48     v[2] = REAL(0.0);
  49     dSafeNormalize3(v);
  50     CHECK_ARRAY_CLOSE(x, v, 3, 1e-6);
  51     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  52
  53     v[0] = REAL(1e-20);
  54     v[1] = REAL(0.0);
  55     v[2] = REAL(0.0);
  56     dSafeNormalize3(v);
  57     CHECK_ARRAY_CLOSE(x, v, 3, 1e-6);
  58     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  59
  60
  61     // Check when value in first component (i.e. [0])
  62     v[0] = REAL(0.0);
  63     v[1] = REAL(1.0);
  64     v[2] = REAL(0.0);
  65     dSafeNormalize3(v);
  66     CHECK_ARRAY_CLOSE(y, v, 3, 1e-6);
  67     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  68
  69     v[0] = REAL(0.0);
  70     v[1] = REAL(0.1);
  71     v[2] = REAL(0.0);
  72     dSafeNormalize3(v);
  73     CHECK_ARRAY_CLOSE(y, v, 3, 1e-6);
  74     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  75
  76     v[0] = REAL(0.0);
  77     v[1] = REAL(1e-20);
  78     v[2] = REAL(0.0);
  79     dSafeNormalize3(v);
  80     CHECK_ARRAY_CLOSE(y, v, 3, 1e-6);
  81     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  82
  83
  84     // Check when value in first component (i.e. [0])
  85     v[0] = REAL(0.0);
  86     v[1] = REAL(0.0);
  87     v[2] = REAL(1.0);
  88     dSafeNormalize3(v);
  89     CHECK_ARRAY_CLOSE(z, v, 3, 1e-6);
  90     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  91
  92     v[0] = REAL(0.0);
  93     v[1] = REAL(0.0);
  94     v[2] = REAL(0.1);
  95     dSafeNormalize3(v);
  96     CHECK_ARRAY_CLOSE(z, v, 3, 1e-6);
  97     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
  98
  99     v[0] = REAL(0.0);
 100     v[1] = REAL(0.0);
 101     v[2] = REAL(1e-20);
 102     dSafeNormalize3(v);
 103     CHECK_ARRAY_CLOSE(z, v, 3, 1e-6);
 104     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 105
 106
 107     // Check negative
 108     // Check when value in first component (i.e. [0])
 109     v[0] = REAL(-1.0);
 110     v[1] = REAL(0.0);
 111     v[2] = REAL(0.0);
 112     dSafeNormalize3(v);
 113     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 114
 115     v[0] = REAL(-0.1);
 116     v[1] = REAL(0.0);
 117     v[2] = REAL(0.0);
 118     dSafeNormalize3(v);
 119     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 120
 121     v[0] = REAL(-1e-20);
 122     v[1] = REAL(0.0);
 123     v[2] = REAL(0.0);
 124     dSafeNormalize3(v);
 125     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 126
 127
 128     // Check when value in first component (i.e. [0])
 129     v[0] = REAL(0.0);
 130     v[1] = REAL(-1.0);
 131     v[2] = REAL(0.0);
 132     dSafeNormalize3(v);
 133     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 134
 135     v[0] = REAL(0.0);
 136     v[1] = REAL(-0.1);
 137     v[2] = REAL(0.0);
 138     dSafeNormalize3(v);
 139     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 140
 141     v[0] = REAL(0.0);
 142     v[1] = REAL(-1e-20);
 143     v[2] = REAL(0.0);
 144     dSafeNormalize3(v);
 145     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 146
 147
 148     // Check when value in first component (i.e. [0])
 149     v[0] = REAL(0.0);
 150     v[1] = REAL(0.0);
 151     v[2] = REAL(-1.0);
 152     dSafeNormalize3(v);
 153     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 154
 155     v[0] = REAL(0.0);
 156     v[1] = REAL(0.0);
 157     v[2] = REAL(-0.1);
 158     dSafeNormalize3(v);
 159     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 160
 161     v[0] = REAL(0.0);
 162     v[1] = REAL(0.0);
 163     v[2] = REAL(-1e-20);
 164     dSafeNormalize3(v);
 165     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 166
 167
 168     v[0] = REAL(9999999999.0);
 169     v[1] = REAL(0.0);
 170     v[2] = REAL(1e-20);
 171     dSafeNormalize3(v);
 172     CHECK_EQUAL(dCalcVectorLength3(v), REAL(1.0));
 173
 174
 175     v[0] = REAL(9999999999.0);
 176     v[1] = REAL(9999.0);
 177     v[2] = REAL(9.0);
 178     dSafeNormalize3(v);
 179     CHECK_CLOSE(dCalcVectorLength3(v), REAL(1.0),REAL(0.001));
 180
 181 }
 182
 183
 184 TEST(test_dOrthogonalizeR)
 185 {
 186     {
 187         dMatrix3 r1 = { 1, 0, 0, 0,
 188                         0, 1, 0, 0,
 189                         0, 0, 1, 0
 190                       };
 191         dMatrix3 r2;
 192         memcpy(r2, r1, sizeof(dMatrix3));
 193         dOrthogonalizeR(r2);
 194         CHECK_ARRAY_EQUAL(r1, r2, 12);
 195     }
 196     {
 197         dMatrix3 r1 = { 0, 1, 0, 0,
 198                         0, 0, 1, 0,
 199                         1, 0, 0, 0
 200                       };
 201         dMatrix3 r2;
 202         memcpy(r2, r1, sizeof(dMatrix3));
 203         dOrthogonalizeR(r2);
 204         CHECK_ARRAY_EQUAL(r1, r2, 12);
 205     }
 206     {
 207         dMatrix3 r1 = { 0, 0, 1, 0,
 208                         1, 0, 0, 0,
 209                         0, 1, 0, 0
 210                       };
 211         dMatrix3 r2;
 212         memcpy(r2, r1, sizeof(dMatrix3));
 213         dOrthogonalizeR(r2);
 214         CHECK_ARRAY_EQUAL(r1, r2, 12);
 215     }
 216     {
 217         dMatrix3 r1 = { -1, 0,  0, 0,
 218                         0, 1,  0, 0,
 219                         0, 0, -1, 0
 220                       };
 221         dMatrix3 r2;
 222         memcpy(r2, r1, sizeof(dMatrix3));
 223         dOrthogonalizeR(r2);
 224         CHECK_ARRAY_EQUAL(r1, r2, 12);
 225     }
 226     {
 227         dMatrix3 r1 = { 0, -1, 0, 0,
 228                         0,  0, 1, 0,
 229                         -1,  0, 0, 0
 230                       };
 231         dMatrix3 r2;
 232         memcpy(r2, r1, sizeof(dMatrix3));
 233         dOrthogonalizeR(r2);
 234         CHECK_ARRAY_EQUAL(r1, r2, 12);
 235     }
 236     {
 237         dMatrix3 r1 = { 0, 0, -1, 0,
 238                         0, -1, 0, 0,
 239                         -1, 0, 0, 0
 240                       };
 241         dMatrix3 r2;
 242         memcpy(r2, r1, sizeof(dMatrix3));
 243         dOrthogonalizeR(r2);
 244         CHECK_ARRAY_EQUAL(r1, r2, 12);
 245     }
 246
 247 }