//------------------------------------------------------------------------
/// Renders the collide model triangle soup.
void TerrainCollisionShape::visualize(Details::OgreOpcodeDebugger* activeDebugger)
{
mActiveDebugger = activeDebugger;
assert(mVertexBuf);
// render the triangle soup
int i, k;
k = 0;
for (i = 0; i < mVerticesPerRow * mVerticesPerRow * 3; i += 9)
{
float v0 = mVertexBuf[i + 0];
float v1 = mVertexBuf[i + 1];
float v2 = mVertexBuf[i + 2];
float v3 = mVertexBuf[i + 3];
float v4 = mVertexBuf[i + 4];
float v5 = mVertexBuf[i + 5];
float v6 = mVertexBuf[i + 6];
float v7 = mVertexBuf[i + 7];
float v8 = mVertexBuf[i + 8];
const Matrix4 &m = getFullTransform();
Vector3 vect0(v0, v1, v2);
Vector3 vect1(v3, v4, v5);
Vector3 vect2(v6, v7, v8);
vect0 = m * vect0;
vect1 = m * vect1;
vect2 = m * vect2;
mActiveDebugger->addShapeLine(vect0.x, vect0.y, vect0.z, vect1.x, vect1.y, vect1.z);
mActiveDebugger->addShapeLine(vect1.x, vect1.y, vect1.z, vect2.x, vect2.y, vect2.z);
mActiveDebugger->addShapeLine(vect2.x, vect2.y, vect2.z, vect0.x, vect0.y, vect0.z);
}
}
void BigIntegerStrassen::FastFourierTransform(vcomp &vect, bool invert){
//static long double PI = 3.14159265358979323846;
static long double PI = 3.14159265358979323846264338327950288419716939937510L;
ll n = (ll)vect.size();
if (n == 1) return;
vcomp vect0(n / 2), vect1(n / 2);
for (ll i = 0, j = 0; i<n; i += 2, ++j){
vect0[j] = vect[i];
vect1[j] = vect[i + 1];
}
FastFourierTransform(vect0, invert);
FastFourierTransform(vect1, invert);
long double ang = 2 * PI / n * (invert ? -1 : 1);
std::complex<long double> w(1), wn(cos(ang), sin(ang));
for (ll i = 0; i < n / 2; ++i) {
vect[i] = vect0[i] + w * vect1[i];
vect[i + n / 2] = vect0[i] - w * vect1[i];
if (invert){
vect[i] /= 2;
vect[i + n / 2] /= 2;
}
w *= wn;
}
}
void llquat_test_object_t::test<12>()
{
LLVector3d vect(-2.0f, 5.0f, -6.0f);
LLQuaternion quat(-3.5f, 4.5f, 3.5f, 6.5f);
LLVector3d result = vect * quat;
ensure(
"1. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed ",
(-633.0f == result.mdV[0]) &&
(-300.0f == result.mdV[1]) &&
(-36.0f == result.mdV[2]));
LLVector3d vect1(5.0f, -4.5f, 8.21f);
LLQuaternion quat1(2.0f, 4.5f, -7.2f, 9.5f);
result = vect1 * quat1;
ensure(
"2. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed",
is_approx_equal_fraction(-120.29f, (F32) result.mdV[0], 8) &&
is_approx_equal_fraction(-1683.958f, (F32) result.mdV[1], 8) &&
is_approx_equal_fraction(516.56f, (F32) result.mdV[2], 8));
LLVector3d vect2(2.0f, 3.5f, 1.1f);
LLQuaternion quat2(1.0f, 4.0f, 2.0f, 5.0f);
result = vect2 * quat2;
ensure(
"3. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed",
is_approx_equal_fraction(18.400001f, (F32) result.mdV[0], 8) &&
is_approx_equal_fraction(188.6f, (F32) result.mdV[1], 8) &&
is_approx_equal_fraction(32.20f, (F32) result.mdV[2], 8));
}
bool ConvexPolygon::isConvex() const
// Algorithme
// effectue le produit vectoriel de tous les cotes
// adjacents
// si ils ont tous le meme signe retourner true
// sinon retourner false
{
Vect start1(pointList[size-1]);
start1 -= pointList[0];
Vect start2(pointList[1]);
start2 -= pointList[0];
double firstProd = start1.x*start2.y - start1.y*start2.x;
for(int i(1); i<size; i++)
{
Vect vect1(pointList[i-1]);
vect1 -= pointList[i];
Vect vect2(pointList[i+1]);
vect2 -= pointList[i];
double prod = vect1.x*vect2.y - vect1.y*vect2.x;
if(prod*firstProd < 0)
{
return false;
}
}
return true;
}
PathPoint Path::update(Vector3<float> refPos, Vector3<float> playerPos)
{
float D_val;
PathPoint current = getCurrent();
PathPoint previous = getPrevious();
float diffX = refPos.x - current.getPosition().x;
float diffY = refPos.y - current.getPosition().y;
float diffZ = refPos.z - current.getPosition().z;
float playerDistFromPlane = 0;
float firstDistFromPlane = 0;
Vector3<float> vect1 (current.getPosition().x - previous.getPosition().x,
current.getPosition().y - previous.getPosition().y,
current.getPosition().z - previous.getPosition().z);
Vector3<float> vect2 (current.getPosition().x + current.getUp().x,
current.getPosition().y + current.getUp().y,
current.getPosition().z + current.getUp().z);
Vector3<float> normal;
float distance = sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
PathPoint firstChoice = getAt(current.getFirstID());
if (distance < RANGE_CHECK) {
if (current.getNumberOfIDs() == 1) {
setChoice(current.getFirstID());
return getCurrent();
}
normal = vect1.Cross(vect2).Normalized();
D_val = (current.getPosition().x * normal.x + current.getPosition().y
* normal.y + current.getPosition().z * normal.z) * -1.0f;
playerDistFromPlane = (normal.x * playerPos.x) +
(normal.y * playerPos.y) + (normal.z * playerPos.z) + D_val;
if (abs(playerDistFromPlane) < MID_BUFFER_WIDTH &&
current.getNumberOfIDs() == 3) {
setChoice(current.getThirdID());
return getCurrent();
}
firstDistFromPlane = normal.x * firstChoice.getPosition().x
+ normal.y * firstChoice.getPosition().y
+ normal.z * firstChoice.getPosition().z + D_val;
if (playerDistFromPlane / abs(playerDistFromPlane) ==
firstDistFromPlane / abs(firstDistFromPlane)) {
setChoice(current.getFirstID());
return getCurrent();
}
else {
setChoice(current.getSecondID());
return getCurrent();
}
}
return getCurrent();
}
void wordlist::mergeSort(vector<string>& unsortedList)
{
if (1 < unsortedList.size())
{
vector<string> vect1(unsortedList.begin(), unsortedList.begin() + unsortedList.size() / 2); // sorts and merges first half of list
mergeSort(vect1);
vector<string> vect2(unsortedList.begin() + unsortedList.size() / 2, unsortedList.end()); // sorts and merges second half of list
mergeSort(vect2);
merge(unsortedList, vect1, vect2);
}
}
void m3math_test_object_t::test<4>()
{
LLMatrix3 llmat3_obj;
LLVector3 vect1(2, 1, 4);
LLVector3 vect2(3, 5, 7);
LLVector3 vect3(6, 9, 7);
llmat3_obj.setRows(vect1, vect2, vect3);
ensure("LLVector3::getFwdRow failed ", vect1 == llmat3_obj.getFwdRow());
ensure("LLVector3::getLeftRow failed ", vect2 == llmat3_obj.getLeftRow());
ensure("LLVector3::getUpRow failed ", vect3 == llmat3_obj.getUpRow());
}
const float TripleProduct( const Vertex& v0, const Vertex& v1, const Vertex& v2 )
{
Vect vect0( v0.x, v0.y, v0.z );
Vect vect1( v1.x, v1.y, v1.z );
Vect vect2( v2.x, v2.y, v2.z );
Vect a = vect2 - vect0;
Vect b = vect1 - vect0;
Vect normal( v0.nx, v0.ny, v0.nz );
return a.cross( b ).dot( normal );
}
sf::Vector2f Collision::calculateDistance(sf::FloatRect box1, sf::FloatRect box2)
{
sf::Vector2f vect1(box1.left, box1.top);
sf::Vector2f vect2(box2.left, box2.top);
sf::Vector2f distance(float(vect1.x - vect2.x), float(vect1.y - vect2.y));
if(distance.y<0)
distance.y *= -1;
if(distance.x<0)
distance.x *= -1;
return distance;
}
void m3math_test_object_t::test<3>()
{
LLMatrix3 llmat3_obj;
LLVector3 vect1(2, 1, 4);
LLVector3 vect2(3, 5, 7);
LLVector3 vect3(6, 9, 7);
llmat3_obj.setRows(vect1, vect2, vect3);
ensure("LLVector3::setRows failed ", 2 == llmat3_obj.mMatrix[0][0] &&
1 == llmat3_obj.mMatrix[0][1] &&
4 == llmat3_obj.mMatrix[0][2] &&
3 == llmat3_obj.mMatrix[1][0] &&
5 == llmat3_obj.mMatrix[1][1] &&
7 == llmat3_obj.mMatrix[1][2] &&
6 == llmat3_obj.mMatrix[2][0] &&
9 == llmat3_obj.mMatrix[2][1] &&
7 == llmat3_obj.mMatrix[2][2]);
}
void llquat_test_object_t::test<3>()
{
LLMatrix3 llmat;
LLVector3 vect1(3.4028234660000000f , 234.56f, 4234.442234f);
LLVector3 vect2(741.434f, 23.00034f, 6567.223423f);
LLVector3 vect3(566.003034f, 12.98705f, 234.764423f);
llmat.setRows(vect1, vect2, vect3);
ensure("LLMatrix3::setRows fn failed.", 3.4028234660000000f == llmat.mMatrix[0][0] &&
234.56f == llmat.mMatrix[0][1] &&
4234.442234f == llmat.mMatrix[0][2] &&
741.434f == llmat.mMatrix[1][0] &&
23.00034f == llmat.mMatrix[1][1] &&
6567.223423f == llmat.mMatrix[1][2] &&
566.003034f == llmat.mMatrix[2][0] &&
12.98705f == llmat.mMatrix[2][1] &&
234.764423f == llmat.mMatrix[2][2]);
}
void llquat_test_object_t::test<11>()
{
LLVector3 vect(12.0f, 5.0f, 60.0f);
LLQuaternion quat(23.5f, 6.5f, 3.23f, 56.5f);
LLVector3 result = vect * quat;
ensure(
"1. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) failed",
is_approx_equal(97182.953125f,result.mV[0]) &&
is_approx_equal(-135405.640625f, result.mV[1]) &&
is_approx_equal(162986.140f, result.mV[2]));
LLVector3 vect1(5.0f, 40.0f, 78.1f);
LLQuaternion quat1(2.034f, 45.5f, 37.23f, 7.5f);
result = vect1 * quat1;
ensure(
"2. LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) failed",
is_approx_equal(33217.703f, result.mV[0]) &&
is_approx_equal(295383.8125f, result.mV[1]) &&
is_approx_equal(84718.140f, result.mV[2]));
}
void CompareTest::checkContainerEquality()
{
std::vector< double > vect1(20);
std::vector< double > vect2(20);
::fwTools::random::fillContainer(0.0001, 10., vect1);
vect2 = vect1;
bool isEqual = ::fwMath::isContainerEqual(vect1, vect2);
CPPUNIT_ASSERT_EQUAL(true, isEqual);
::boost::uint32_t seedVal = std::time(NULL);
::fwTools::random::fillContainer(0.000001, 0.000009, vect1, seedVal);
::fwTools::random::fillContainer(0.000001, 0.000009, vect2, ++seedVal);
isEqual = ::fwMath::isContainerEqual(vect1, vect2);
CPPUNIT_ASSERT_EQUAL(true, isEqual);
isEqual = ::fwMath::isContainerEqual(vect1, vect2, 0.0000001f);
CPPUNIT_ASSERT_EQUAL(false, isEqual);
}
void llquat_test_object_t::test<10>()
{
LLVector4 vect(12.0f, 5.0f, 60.0f, 75.1f);
LLQuaternion quat(2323.034f, 23.5f, 673.23f, 57667.5f);
LLVector4 result = vect * quat;
ensure(
"1. LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot) failed",
is_approx_equal(39928406016.0f, result.mV[0]) &&
// gcc on x86 actually gives us more precision than we were expecting, verified with -ffloat-store - we forgive this
(1457802240.0f >= result.mV[1]) && // gcc+x86+linux
(1457800960.0f <= result.mV[1]) && // elsewhere
is_approx_equal(200580612096.0f, result.mV[2]) &&
(75.099998f == result.mV[3]));
LLVector4 vect1(22.0f, 45.0f, 40.0f, 78.1f);
LLQuaternion quat1(2.034f, 45.5f, 37.23f, 7.5f);
result = vect1 * quat1;
ensure(
"2. LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot) failed",
is_approx_equal(-58153.5390f, result.mV[0]) &&
(183787.8125f == result.mV[1]) &&
(116864.164063f == result.mV[2]) &&
(78.099998f == result.mV[3]));
}
int main(int argc, char **argv)
{
bool fail = false;
//
// now create a domain and a modelbuilder
// and build the model
//
FEM_ObjectBroker theBroker;
Domain *theDomain = new Domain();
FileDatastore *theDatabase
= new FileDatastore("/tmp/database/test1",*theDomain,theBroker);
FileDatastore &theDb = *theDatabase;
opserr << "TESTING IDs: \n";
ID id1(2);
id1(0) = 1;
id1(1) = 1;
ID id0(2);
id0(0) = 0;
id0(1) = 0;
ID id2(2);
id2(0) = 2;
id2(1) = 2;
ID id3(2);
id3(0) = 3;
id3(1) = 3;
ID id4(2);
id4(0) = 4;
id4(1) = 4;
ID id5(2);
id5(0) = 5;
id5(1) = 5;
ID id6(2);
id6(0) = 6;
id6(1) = 6;
theDb.sendID(1,1,id1);
theDb.sendID(1,2,id2);
theDb.sendID(2,1,id3);
theDb.sendID(2,2,id4);
opserr << "RESULTS\n";
ID recvID(2);
theDb.recvID(1,1,recvID);
opserr << "1: " << recvID;
theDb.recvID(1,2,recvID);
opserr << "2: " << recvID;
theDb.recvID(2,1,recvID);
opserr << "3: " << recvID;
theDb.recvID(2,2,recvID);
opserr << "4: " << recvID;
theDb.sendID(1,1,id1);
theDb.sendID(3,1,id3);
theDb.sendID(2,1,id2);
theDb.sendID(0,1,id0);
theDb.sendID(1,2,id1);
theDb.sendID(2,2,id2);
theDb.sendID(3,1,id3);
theDb.sendID(5,1,id5);
theDb.sendID(4,1,id4);
theDb.sendID(1,1,id1);
theDb.recvID(3,1,id5);
opserr << "3: " << id5;
theDb.recvID(1,1,id5);
opserr << "1: " << id5;
theDb.recvID(2,1,id5);
opserr << "2: " << id5;
theDb.recvID(1,2,id5);
opserr << "1: " << id5;
theDb.recvID(2,2,id5);
opserr << "3: " << id5;
theDb.recvID(3,1,id5);
opserr << "3: " << id5;
theDb.recvID(4,1,id5);
opserr << "4: " << id5;
theDb.recvID(5,1,id5);
opserr << "5: " << id5;
opserr << "FAILURE: " << theDb.recvID(6,1,id5) << " returned\n";
opserr << "FAILURE " << theDb.recvID(6,1,id5) << " returned\n";
theDb.recvID(0,1,id5);
opserr << "0: " << id5;
theDb.recvID(5,1,id5);
opserr << "5: " << id5;
ID id64(4);
id64(0) = 6;
id64(1) = 6;
id64(2) = 6;
id64(3) = 6;
theDb.sendID(6,1,id64);
theDb.recvID(6,1,id64);
opserr << id64;
opserr << "TESTING MATRICES: \n";
Matrix mat1(2,2);
mat1(0,0) = 1.1;
mat1(0,1) = 11.1;
mat1(1,0) = 111.1;
mat1(1,1) = 1111.1;
Matrix mat2(2,2);
mat2(0,0) = 2.2;
mat2(0,1) = 22.2;
mat2(1,0) = 222.2;
mat2(1,1) = 2222.2;
theDb.sendMatrix(2,1,mat2);
theDb.sendMatrix(1,1,mat1);
theDb.sendMatrix(3,2,mat2);
theDb.sendMatrix(3,1,mat1);
Matrix mat3(2,2);
theDb.recvMatrix(1,1,mat3);
opserr << mat1 << mat3 << endln;
theDb.recvMatrix(2,1,mat3);
opserr << mat2 << mat3 << endln;
theDb.recvMatrix(3,2,mat3);
opserr << mat2 << mat3 << endln;
theDb.recvMatrix(3,1,mat3);
opserr << mat1 << mat3 << endln;
// theDb.sendMatrix(2,1,mat1);
theDb.recvMatrix(2,1,mat3);
opserr << mat2 << mat3;
opserr << "TESTING VECTORS: \n";
Vector vect1(2);
vect1(0) = 1.1;
vect1(1) = 2.22;
Vector vect2(2);
vect2(0) = 3;
vect2(1) = 4.2;
Vector vect3(2);
vect3(0) = 5;
vect3(1) = 6;
Vector vect4(2);
vect4(0) = 7;
vect4(1) = 8.8e12;
theDb.sendVector(1,1,vect1);
theDb.sendVector(1,2,vect2);
theDb.sendVector(2,1,vect3);
theDb.sendVector(2,2,vect4);
opserr << "RESULTS\n";
Vector vect5(2);
theDb.recvVector(1,1,vect5);
opserr << vect1 << vect5 << endln;
theDb.recvVector(1,2,vect5);
opserr << vect2 << vect5 << endln;
theDb.recvVector(2,1,vect5);
opserr << vect3 << vect5 << endln;
theDb.recvVector(2,2,vect5);
opserr << vect4 << vect5 << endln;
theDb.sendVector(2,2,vect1);
theDb.sendVector(2,1,vect2);
theDb.sendVector(1,2,vect3);
theDb.sendVector(1,1,vect4);
theDb.recvVector(1,1,vect5);
opserr << vect4 << vect5 << endln;
theDb.recvVector(1,2,vect5);
opserr << vect3 << vect5 << endln;
theDb.recvVector(2,1,vect5);
opserr << vect2 << vect5 << endln;
theDb.recvVector(2,2,vect5);
opserr << vect1 << vect5 << endln;
theDb.sendVector(4,4,vect5);
theDb.recvVector(4,4,vect5);
opserr << vect5 << vect5 << endln;
theDb.recvVector(5,5,vect5);
opserr << "FAIL\n";
theDatabase->commitState(0);
/* */
/*
theDb.sendID(2,2,id1);
theDb.sendID(2,1,id2);
theDb.sendID(1,2,id3);
theDb.sendID(1,1,id4);
theDb.recvID(1,1,id5);
opserr << id5;
theDb.recvID(1,2,id5);
opserr << id5;
theDb.recvID(2,1,id5);
opserr << id5;
theDb.recvID(2,2,id5);
opserr << id5;
theDb.sendID(4,4,id5);
theDb.recvID(4,4,id5);
opserr << id5;
theDb.recvID(5,5,id5);
opserr << id5;
*/
/**************************
**************************/
/*
*/
// theModelBuilder->buildFE_Model();
// theDb.commitState(0);
// theDb.restoreState(0);
// theDb.restoreElements(0);
/*
theDb.restoreNode(1,0);
theDb.restoreNode(2,0);
theDb.restoreNode(3,0);
theDb.restoreNode(4,0);
theDb.restoreElement(0,0);
theDb.restoreElement(1,0);
theDb.restoreElement(2,0);
*/
// opserr << *theDomain;
delete theDatabase;
exit(0);
}
