UnitTests/BulletUnitTests/TestLinearMath.h

   1 #ifndef TEST_LINEAR_MATH_HAS_BEEN_INCLUDED\r
   2 #define TEST_LINEAR_MATH_HAS_BEEN_INCLUDED\r
   3 \r
   4 #include "cppunit/TestFixture.h"\r
   5 #include "cppunit/extensions/HelperMacros.h"\r
   6 \r
   7 #include "LinearMath/btScalar.h"\r
   8 \r
   9 #define TEST_NUM_UNITSPHERE_POINTS 42\r
  10 \r
  11 namespace\r
  12 {\r
  13   struct compLess\r
  14   {\r
  15     bool operator()(const int& p1, const int& p2)  const\r
  16     {\r
  17       return p1 < p2;\r
  18     }\r
  19   };\r
  20 }\r
  21 \r
  22 static btVector3        sPenetrationDirections[TEST_NUM_UNITSPHERE_POINTS] = \r
  23         {\r
  24         btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),\r
  25         btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),\r
  26         btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),\r
  27         btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),\r
  28         btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),\r
  29         btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),\r
  30         btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),\r
  31         btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),\r
  32         btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),\r
  33         btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),\r
  34         btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),\r
  35         btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),\r
  36         btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),\r
  37         btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),\r
  38         btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),\r
  39         btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),\r
  40         btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),\r
  41         btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),\r
  42         btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),\r
  43         btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),\r
  44         btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),\r
  45         btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),\r
  46         btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),\r
  47         btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),\r
  48         btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),\r
  49         btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),\r
  50         btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),\r
  51         btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),\r
  52         btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),\r
  53         btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),\r
  54         btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),\r
  55         btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),\r
  56         btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),\r
  57         btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),\r
  58         btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),\r
  59         btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),\r
  60         btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),\r
  61         btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),\r
  62         btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),\r
  63         btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),\r
  64         btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),\r
  65         btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))\r
  66         };\r
  67 \r
  68 \r
  69 \r
  70 // ---------------------------------------------------------------------------\r
  71 \r
  72 class TestLinearMath : public CppUnit::TestFixture\r
  73 {\r
  74 \r
  75         \r
  76 \r
  77 public:\r
  78 \r
  79         void setUp() \r
  80         {\r
  81         }\r
  82 \r
  83         void tearDown() \r
  84         {\r
  85         }\r
  86 \r
  87 \r
  88         void testNormalize()\r
  89         {\r
  90                 \r
  91                 const btVector3 xaxis(1,0,0);\r
  92                 const btVector3 yaxis(0,1,0);\r
  93                 const btVector3 zaxis(0,0,1);\r
  94                 \r
  95                 const btVector3 negxaxis(-1,0,0);\r
  96                 const btVector3 negyaxis(0,-1,0);\r
  97                 const btVector3 negzaxis(0,0,-1);\r
  98 \r
  99                 btVector3 vec;\r
 100                 \r
 101                 vec.setValue(1e-20,0,0);\r
 102                 vec.safeNormalize();\r
 103                 CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-6 );\r
 104 \r
 105                 vec.setValue(1e20,0,0);\r
 106                 vec.safeNormalize();\r
 107                 CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-6 );\r
 108 \r
 109                 //vec.setValue(1e-20,0,0);\r
 110                 //vec.normalize();\r
 111                 //CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 );\r
 112 \r
 113                 vec.setValue(1e19,0,0);\r
 114                 vec.normalize();\r
 115                 CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.0, vec.length2(), 1e-5 );\r
 116                 \r
 117         }\r
 118         \r
 119         void testQuicksort()\r
 120         {\r
 121                 int tests = 0;\r
 122                 int numElems = 100;\r
 123                 btAlignedObjectArray<int> m_unsortedIntegers;\r
 124                 m_unsortedIntegers.resize(numElems);\r
 125                 for (int i=0;i<numElems;i++)\r
 126                 {\r
 127                         m_unsortedIntegers[i] = i;\r
 128                 }\r
 129 \r
 130                 m_unsortedIntegers.quickSort(::compLess());\r
 131                 for (int i=1;i<numElems;i++)\r
 132                 {\r
 133                         CPPUNIT_ASSERT(m_unsortedIntegers[i-1]<=m_unsortedIntegers[i]);\r
 134                 }\r
 135                 for (int i=0;i<numElems;i++)\r
 136                 {\r
 137                         m_unsortedIntegers[i] = numElems-i;\r
 138                 }\r
 139                 m_unsortedIntegers.quickSort(::compLess());\r
 140                 for (int i=1;i<numElems;i++)\r
 141                 {\r
 142                         CPPUNIT_ASSERT(m_unsortedIntegers[i-1]<=m_unsortedIntegers[i]);\r
 143                 }\r
 144                 \r
 145                 \r
 146         }\r
 147         \r
 148 \r
 149         void testQuaternionGetAxisAngle()\r
 150         {\r
 151                 int steps=100;\r
 152 \r
 153                 for (int j=0;j<TEST_NUM_UNITSPHERE_POINTS;j++)\r
 154                 {\r
 155                         btVector3 axis = sPenetrationDirections[j];\r
 156 \r
 157                         for (int i=-steps+1;i<steps;i++)\r
 158                         {\r
 159                                 btScalar angle=i*SIMD_2_PI/btScalar(steps);\r
 160 \r
 161                                 btQuaternion quat(axis,angle);\r
 162                                 btScalar compAngle = quat.getAngle();\r
 163                                 if (i>=0)\r
 164                                         CPPUNIT_ASSERT_DOUBLES_EQUAL( angle, compAngle, 1e-5 );\r
 165                                 else\r
 166                                 {\r
 167                                         CPPUNIT_ASSERT_DOUBLES_EQUAL( btFabs(angle), btFabs(compAngle), 1e-5 );\r
 168                                 }\r
 169                                 btVector3 compAxis = quat.getAxis();\r
 170 \r
 171                                 if (compAngle>SIMD_EPSILON)\r
 172                                 {\r
 173                                         if (i>=0)\r
 174                                         {\r
 175                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getX(), compAxis.getX(), 1e-4 );\r
 176                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getY(), compAxis.getY(), 1e-4 );\r
 177                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( axis.getZ(), compAxis.getZ(), 1e-4 );\r
 178                                         } else\r
 179                                         {\r
 180                                                 btScalar sign = compAngle*angle<0? -1 : 1;\r
 181                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getX(), compAxis.getX(), 1e-4 );\r
 182                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getY(), compAxis.getY(), 1e-4 );\r
 183                                                 CPPUNIT_ASSERT_DOUBLES_EQUAL( sign*axis.getZ(), compAxis.getZ(), 1e-4 );\r
 184                                         }\r
 185                                 } else\r
 186                                 {\r
 187                                         CPPUNIT_ASSERT_DOUBLES_EQUAL( 1.f, compAxis.getX(), 1e-4 );\r
 188                                         CPPUNIT_ASSERT_DOUBLES_EQUAL( 0.f, compAxis.getY(), 1e-4 );\r
 189                                         CPPUNIT_ASSERT_DOUBLES_EQUAL( 0.f, compAxis.getZ(), 1e-4 );\r
 190                                 \r
 191                                 }\r
 192                         }\r
 193                 }\r
 194         }\r
 195 \r
 196 \r
 197         \r
 198         \r
 199         CPPUNIT_TEST_SUITE(TestLinearMath);\r
 200         CPPUNIT_TEST(testQuicksort);\r
 201         CPPUNIT_TEST(testNormalize);\r
 202         CPPUNIT_TEST(testQuaternionGetAxisAngle);\r
 203         \r
 204         CPPUNIT_TEST_SUITE_END();\r
 205 \r
 206 private:\r
 207 \r
 208 };\r
 209 \r
 210 #endif //TEST_LINEAR_MATH_HAS_BEEN_INCLUDED\r