171 lines
4.4 KiB
C++
171 lines
4.4 KiB
C++
//
|
|
// btReducedVector.cpp
|
|
// LinearMath
|
|
//
|
|
// Created by Xuchen Han on 4/4/20.
|
|
//
|
|
#include <stdio.h>
|
|
#include "btReducedVector.h"
|
|
#include <cmath>
|
|
|
|
// returns the projection of this onto other
|
|
btReducedVector btReducedVector::proj(const btReducedVector& other) const
|
|
{
|
|
btReducedVector ret(m_sz);
|
|
btScalar other_length2 = other.length2();
|
|
if (other_length2 < SIMD_EPSILON)
|
|
{
|
|
return ret;
|
|
}
|
|
return other*(this->dot(other))/other_length2;
|
|
}
|
|
|
|
void btReducedVector::normalize()
|
|
{
|
|
if (this->length2() < SIMD_EPSILON)
|
|
{
|
|
m_indices.clear();
|
|
m_vecs.clear();
|
|
return;
|
|
}
|
|
*this /= std::sqrt(this->length2());
|
|
}
|
|
|
|
bool btReducedVector::testAdd() const
|
|
{
|
|
int sz = 5;
|
|
btAlignedObjectArray<int> id1;
|
|
id1.push_back(1);
|
|
id1.push_back(3);
|
|
btAlignedObjectArray<btVector3> v1;
|
|
v1.push_back(btVector3(1,0,1));
|
|
v1.push_back(btVector3(3,1,5));
|
|
btAlignedObjectArray<int> id2;
|
|
id2.push_back(2);
|
|
id2.push_back(3);
|
|
id2.push_back(5);
|
|
btAlignedObjectArray<btVector3> v2;
|
|
v2.push_back(btVector3(2,3,1));
|
|
v2.push_back(btVector3(3,4,9));
|
|
v2.push_back(btVector3(0,4,0));
|
|
btAlignedObjectArray<int> id3;
|
|
id3.push_back(1);
|
|
id3.push_back(2);
|
|
id3.push_back(3);
|
|
id3.push_back(5);
|
|
btAlignedObjectArray<btVector3> v3;
|
|
v3.push_back(btVector3(1,0,1));
|
|
v3.push_back(btVector3(2,3,1));
|
|
v3.push_back(btVector3(6,5,14));
|
|
v3.push_back(btVector3(0,4,0));
|
|
btReducedVector rv1(sz, id1, v1);
|
|
btReducedVector rv2(sz, id2, v2);
|
|
btReducedVector ans(sz, id3, v3);
|
|
bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
|
|
if (!ret)
|
|
printf("btReducedVector testAdd failed\n");
|
|
return ret;
|
|
}
|
|
|
|
bool btReducedVector::testMinus() const
|
|
{
|
|
int sz = 5;
|
|
btAlignedObjectArray<int> id1;
|
|
id1.push_back(1);
|
|
id1.push_back(3);
|
|
btAlignedObjectArray<btVector3> v1;
|
|
v1.push_back(btVector3(1,0,1));
|
|
v1.push_back(btVector3(3,1,5));
|
|
btAlignedObjectArray<int> id2;
|
|
id2.push_back(2);
|
|
id2.push_back(3);
|
|
id2.push_back(5);
|
|
btAlignedObjectArray<btVector3> v2;
|
|
v2.push_back(btVector3(2,3,1));
|
|
v2.push_back(btVector3(3,4,9));
|
|
v2.push_back(btVector3(0,4,0));
|
|
btAlignedObjectArray<int> id3;
|
|
id3.push_back(1);
|
|
id3.push_back(2);
|
|
id3.push_back(3);
|
|
id3.push_back(5);
|
|
btAlignedObjectArray<btVector3> v3;
|
|
v3.push_back(btVector3(-1,-0,-1));
|
|
v3.push_back(btVector3(2,3,1));
|
|
v3.push_back(btVector3(0,3,4));
|
|
v3.push_back(btVector3(0,4,0));
|
|
btReducedVector rv1(sz, id1, v1);
|
|
btReducedVector rv2(sz, id2, v2);
|
|
btReducedVector ans(sz, id3, v3);
|
|
bool ret = (ans == rv2-rv1);
|
|
if (!ret)
|
|
printf("btReducedVector testMinus failed\n");
|
|
return ret;
|
|
}
|
|
|
|
bool btReducedVector::testDot() const
|
|
{
|
|
int sz = 5;
|
|
btAlignedObjectArray<int> id1;
|
|
id1.push_back(1);
|
|
id1.push_back(3);
|
|
btAlignedObjectArray<btVector3> v1;
|
|
v1.push_back(btVector3(1,0,1));
|
|
v1.push_back(btVector3(3,1,5));
|
|
btAlignedObjectArray<int> id2;
|
|
id2.push_back(2);
|
|
id2.push_back(3);
|
|
id2.push_back(5);
|
|
btAlignedObjectArray<btVector3> v2;
|
|
v2.push_back(btVector3(2,3,1));
|
|
v2.push_back(btVector3(3,4,9));
|
|
v2.push_back(btVector3(0,4,0));
|
|
btReducedVector rv1(sz, id1, v1);
|
|
btReducedVector rv2(sz, id2, v2);
|
|
btScalar ans = 58;
|
|
bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
|
|
ans = 14+16+9+16+81;
|
|
ret &= (ans==rv2.dot(rv2));
|
|
|
|
if (!ret)
|
|
printf("btReducedVector testDot failed\n");
|
|
return ret;
|
|
}
|
|
|
|
bool btReducedVector::testMultiply() const
|
|
{
|
|
int sz = 5;
|
|
btAlignedObjectArray<int> id1;
|
|
id1.push_back(1);
|
|
id1.push_back(3);
|
|
btAlignedObjectArray<btVector3> v1;
|
|
v1.push_back(btVector3(1,0,1));
|
|
v1.push_back(btVector3(3,1,5));
|
|
btScalar s = 2;
|
|
btReducedVector rv1(sz, id1, v1);
|
|
btAlignedObjectArray<int> id2;
|
|
id2.push_back(1);
|
|
id2.push_back(3);
|
|
btAlignedObjectArray<btVector3> v2;
|
|
v2.push_back(btVector3(2,0,2));
|
|
v2.push_back(btVector3(6,2,10));
|
|
btReducedVector ans(sz, id2, v2);
|
|
bool ret = (ans == rv1*s);
|
|
if (!ret)
|
|
printf("btReducedVector testMultiply failed\n");
|
|
return ret;
|
|
}
|
|
|
|
void btReducedVector::test() const
|
|
{
|
|
bool ans = testAdd() && testMinus() && testDot() && testMultiply();
|
|
if (ans)
|
|
{
|
|
printf("All tests passed\n");
|
|
}
|
|
else
|
|
{
|
|
printf("Tests failed\n");
|
|
}
|
|
}
|