dali-physics/third-party/bullet3/src/LinearMath/btReducedVector.cpp

   1 //
   2 //  btReducedVector.cpp
   3 //  LinearMath
   4 //
   5 //  Created by Xuchen Han on 4/4/20.
   6 //
   7 #include <stdio.h>
   8 #include "btReducedVector.h"
   9 #include <cmath>
  10
  11 // returns the projection of this onto other
  12 btReducedVector btReducedVector::proj(const btReducedVector& other) const
  13 {
  14     btReducedVector ret(m_sz);
  15     btScalar other_length2 = other.length2();
  16     if (other_length2 < SIMD_EPSILON)
  17     {
  18         return ret;
  19     }
  20     return other*(this->dot(other))/other_length2;
  21 }
  22
  23 void btReducedVector::normalize()
  24 {
  25     if (this->length2() < SIMD_EPSILON)
  26     {
  27         m_indices.clear();
  28         m_vecs.clear();
  29         return;
  30     }
  31     *this /= std::sqrt(this->length2());
  32 }
  33
  34 bool btReducedVector::testAdd() const
  35 {
  36     int sz = 5;
  37     btAlignedObjectArray<int> id1;
  38     id1.push_back(1);
  39     id1.push_back(3);
  40     btAlignedObjectArray<btVector3> v1;
  41     v1.push_back(btVector3(1,0,1));
  42     v1.push_back(btVector3(3,1,5));
  43     btAlignedObjectArray<int> id2;
  44     id2.push_back(2);
  45     id2.push_back(3);
  46     id2.push_back(5);
  47     btAlignedObjectArray<btVector3> v2;
  48     v2.push_back(btVector3(2,3,1));
  49     v2.push_back(btVector3(3,4,9));
  50     v2.push_back(btVector3(0,4,0));
  51     btAlignedObjectArray<int> id3;
  52     id3.push_back(1);
  53     id3.push_back(2);
  54     id3.push_back(3);
  55     id3.push_back(5);
  56     btAlignedObjectArray<btVector3> v3;
  57     v3.push_back(btVector3(1,0,1));
  58     v3.push_back(btVector3(2,3,1));
  59     v3.push_back(btVector3(6,5,14));
  60     v3.push_back(btVector3(0,4,0));
  61     btReducedVector rv1(sz, id1, v1);
  62     btReducedVector rv2(sz, id2, v2);
  63     btReducedVector ans(sz, id3, v3);
  64     bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
  65     if (!ret)
  66         printf("btReducedVector testAdd failed\n");
  67     return ret;
  68 }
  69
  70 bool btReducedVector::testMinus() const
  71 {
  72     int sz = 5;
  73     btAlignedObjectArray<int> id1;
  74     id1.push_back(1);
  75     id1.push_back(3);
  76     btAlignedObjectArray<btVector3> v1;
  77     v1.push_back(btVector3(1,0,1));
  78     v1.push_back(btVector3(3,1,5));
  79     btAlignedObjectArray<int> id2;
  80     id2.push_back(2);
  81     id2.push_back(3);
  82     id2.push_back(5);
  83     btAlignedObjectArray<btVector3> v2;
  84     v2.push_back(btVector3(2,3,1));
  85     v2.push_back(btVector3(3,4,9));
  86     v2.push_back(btVector3(0,4,0));
  87     btAlignedObjectArray<int> id3;
  88     id3.push_back(1);
  89     id3.push_back(2);
  90     id3.push_back(3);
  91     id3.push_back(5);
  92     btAlignedObjectArray<btVector3> v3;
  93     v3.push_back(btVector3(-1,-0,-1));
  94     v3.push_back(btVector3(2,3,1));
  95     v3.push_back(btVector3(0,3,4));
  96     v3.push_back(btVector3(0,4,0));
  97     btReducedVector rv1(sz, id1, v1);
  98     btReducedVector rv2(sz, id2, v2);
  99     btReducedVector ans(sz, id3, v3);
 100     bool ret = (ans == rv2-rv1);
 101     if (!ret)
 102         printf("btReducedVector testMinus failed\n");
 103     return ret;
 104 }
 105
 106 bool btReducedVector::testDot() const
 107 {
 108     int sz = 5;
 109     btAlignedObjectArray<int> id1;
 110     id1.push_back(1);
 111     id1.push_back(3);
 112     btAlignedObjectArray<btVector3> v1;
 113     v1.push_back(btVector3(1,0,1));
 114     v1.push_back(btVector3(3,1,5));
 115     btAlignedObjectArray<int> id2;
 116     id2.push_back(2);
 117     id2.push_back(3);
 118     id2.push_back(5);
 119     btAlignedObjectArray<btVector3> v2;
 120     v2.push_back(btVector3(2,3,1));
 121     v2.push_back(btVector3(3,4,9));
 122     v2.push_back(btVector3(0,4,0));
 123     btReducedVector rv1(sz, id1, v1);
 124     btReducedVector rv2(sz, id2, v2);
 125     btScalar ans = 58;
 126     bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
 127     ans = 14+16+9+16+81;
 128     ret &= (ans==rv2.dot(rv2));
 129
 130     if (!ret)
 131         printf("btReducedVector testDot failed\n");
 132     return ret;
 133 }
 134
 135 bool btReducedVector::testMultiply() const
 136 {
 137     int sz = 5;
 138     btAlignedObjectArray<int> id1;
 139     id1.push_back(1);
 140     id1.push_back(3);
 141     btAlignedObjectArray<btVector3> v1;
 142     v1.push_back(btVector3(1,0,1));
 143     v1.push_back(btVector3(3,1,5));
 144     btScalar s = 2;
 145     btReducedVector rv1(sz, id1, v1);
 146     btAlignedObjectArray<int> id2;
 147     id2.push_back(1);
 148     id2.push_back(3);
 149     btAlignedObjectArray<btVector3> v2;
 150     v2.push_back(btVector3(2,0,2));
 151     v2.push_back(btVector3(6,2,10));
 152     btReducedVector ans(sz, id2, v2);
 153     bool ret = (ans == rv1*s);
 154     if (!ret)
 155         printf("btReducedVector testMultiply failed\n");
 156     return ret;
 157 }
 158
 159 void btReducedVector::test() const
 160 {
 161     bool ans = testAdd() && testMinus() && testDot() && testMultiply();
 162     if (ans)
 163     {
 164         printf("All tests passed\n");
 165     }
 166     else
 167     {
 168         printf("Tests failed\n");
 169     }
 170 }