void MeshMassPropertiesTests::testClosedTetrahedronMesh() {
// given a tetrahedron as a closed mesh of four tiangles
// verify MeshMassProperties computes the right nubers
// these numbers from the Tonon paper:
VectorOfPoints points;
points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f));
points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f));
points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f));
points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f));
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
btVector3 expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
VectorOfIndices triangles;
pushTriangle(triangles, 0, 2, 1);
pushTriangle(triangles, 0, 3, 2);
pushTriangle(triangles, 0, 1, 3);
pushTriangle(triangles, 1, 2, 3);
// compute mass properties
MeshMassProperties mesh(points, triangles);
// verify
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
// test again, but this time shift the points so that the origin is definitely OUTSIDE the mesh
btVector3 shift = points[0] + expectedCenterOfMass;
for (int i = 0; i < (int)points.size(); ++i) {
points[i] += shift;
}
expectedCenterOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
// compute mass properties
mesh.computeMassProperties(points, triangles);
// verify
// QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
// QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
// QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
}
void TransformTests::getMatrix() {
const vec3 t(0.0f, 0.0f, 10.0f);
// create a matrix that is composed of a PI/2 rotation followed by a small z translation
const mat4 m(vec4(rot90 * xAxis, 0.0f),
vec4(rot90 * yAxis, 0.0f),
vec4(rot90 * zAxis, 0.0f),
vec4(vec4(t, 1.0f)));
// postScale by a mirror about the x axis.
const mat4 mirrorX(vec4(-1.0f, 0.0f, 0.0f, 0.0f),
vec4( 0.0f, 1.0f, 0.0f, 0.0f),
vec4( 0.0f, 0.0f, 1.0f, 0.0f),
vec4( 0.0f, 0.0f, 0.0f, 1.0f));
const mat4 result_a = m * mirrorX;
Transform xform;
xform.setRotation(rot90);
xform.setTranslation(t);
xform.postScale(vec3(-1.0f, 1.0f, 1.0f));
mat4 result_b;
xform.getMatrix(result_b);
QCOMPARE_WITH_ABS_ERROR(result_a, result_b, TEST_EPSILON);
}
static void testQuatCompression(glm::quat testQuat) {
float MAX_COMPONENT_ERROR = 4.3e-5f;
glm::quat q;
uint8_t bytes[6];
packOrientationQuatToSixBytes(bytes, testQuat);
unpackOrientationQuatFromSixBytes(bytes, q);
if (glm::dot(q, testQuat) < 0.0f) {
q = -q;
}
QCOMPARE_WITH_ABS_ERROR(q.x, testQuat.x, MAX_COMPONENT_ERROR);
QCOMPARE_WITH_ABS_ERROR(q.y, testQuat.y, MAX_COMPONENT_ERROR);
QCOMPARE_WITH_ABS_ERROR(q.z, testQuat.z, MAX_COMPONENT_ERROR);
QCOMPARE_WITH_ABS_ERROR(q.w, testQuat.w, MAX_COMPONENT_ERROR);
}
void GLMHelpersTests::testEulerDecomposition() {
// quat to euler and back again....
const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f));
const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f));
const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f));
const float EPSILON = 0.00001f;
std::vector<glm::quat> quatVec = {
glm::quat(),
ROT_X_90,
ROT_Y_180,
ROT_Z_30,
ROT_X_90 * ROT_Y_180 * ROT_Z_30,
ROT_X_90 * ROT_Z_30 * ROT_Y_180,
ROT_Y_180 * ROT_Z_30 * ROT_X_90,
ROT_Y_180 * ROT_X_90 * ROT_Z_30,
ROT_Z_30 * ROT_X_90 * ROT_Y_180,
ROT_Z_30 * ROT_Y_180 * ROT_X_90,
};
for (auto& q : quatVec) {
glm::vec3 euler = safeEulerAngles(q);
glm::quat r(euler);
// when the axis and angle are flipped.
if (glm::dot(q, r) < 0.0f) {
r = -r;
}
QCOMPARE_WITH_ABS_ERROR(q, r, EPSILON);
}
}
void MeshMassPropertiesTests::testParallelAxisTheorem() {
// verity we can compute the inertia tensor of a box in two different ways:
// (a) as one box
// (b) as a combination of two partial boxes.
btScalar bigBoxX = 7.0f;
btScalar bigBoxY = 9.0f;
btScalar bigBoxZ = 11.0f;
btScalar bigBoxMass = bigBoxX * bigBoxY * bigBoxZ;
btMatrix3x3 bitBoxInertia;
computeBoxInertia(bigBoxMass, btVector3(bigBoxX, bigBoxY, bigBoxZ), bitBoxInertia);
btScalar smallBoxX = bigBoxX / 2.0f;
btScalar smallBoxY = bigBoxY;
btScalar smallBoxZ = bigBoxZ;
btScalar smallBoxMass = smallBoxX * smallBoxY * smallBoxZ;
btMatrix3x3 smallBoxI;
computeBoxInertia(smallBoxMass, btVector3(smallBoxX, smallBoxY, smallBoxZ), smallBoxI);
btVector3 smallBoxOffset(smallBoxX / 2.0f, 0.0f, 0.0f);
btMatrix3x3 smallBoxShiftedRight = smallBoxI;
applyParallelAxisTheorem(smallBoxShiftedRight, smallBoxOffset, smallBoxMass);
btMatrix3x3 smallBoxShiftedLeft = smallBoxI;
applyParallelAxisTheorem(smallBoxShiftedLeft, -smallBoxOffset, smallBoxMass);
btMatrix3x3 twoSmallBoxesInertia = smallBoxShiftedRight + smallBoxShiftedLeft;
QCOMPARE_WITH_ABS_ERROR(bitBoxInertia, twoSmallBoxesInertia, acceptableAbsoluteError);
}
void MeshMassPropertiesTests::testOpenTetrahedonMesh() {
// given the simplest possible mesh (open, with one triangle)
// verify MeshMassProperties computes the right nubers
// these numbers from the Tonon paper:
VectorOfPoints points;
points.push_back(btVector3(8.33220f, -11.86875f, 0.93355f));
points.push_back(btVector3(0.75523f, 5.00000f, 16.37072f));
points.push_back(btVector3(52.61236f, 5.00000f, -5.38580f));
points.push_back(btVector3(2.00000f, 5.00000f, 3.00000f));
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
// test as an open mesh with one triangle
VectorOfPoints shiftedPoints;
shiftedPoints.push_back(points[0] - points[0]);
shiftedPoints.push_back(points[1] - points[0]);
shiftedPoints.push_back(points[2] - points[0]);
shiftedPoints.push_back(points[3] - points[0]);
VectorOfIndices triangles;
pushTriangle(triangles, 1, 2, 3);
btVector3 expectedCenterOfMass = 0.25f * (shiftedPoints[0] + shiftedPoints[1] + shiftedPoints[2] + shiftedPoints[3]);
// compute mass properties
MeshMassProperties mesh(shiftedPoints, triangles);
// verify
// (expected - actual) / expected > e ==> expected - actual > e * expected
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
}
void TransformTests::getInverseMatrix() {
const vec3 t(0.0f, 0.0f, 10.0f);
// create a matrix that is composed of a PI/2 rotation followed by a small z translation
const mat4 m(vec4(rot90 * xAxis, 0.0f),
vec4(rot90 * yAxis, 0.0f),
vec4(rot90 * zAxis, 0.0f),
vec4(vec4(t, 1.0f)));
// mirror about the x axis.
const mat4 mirrorX(vec4(-1.0f, 0.0f, 0.0f, 0.0f),
vec4( 0.0f, 1.0f, 0.0f, 0.0f),
vec4( 0.0f, 0.0f, 1.0f, 0.0f),
vec4( 0.0f, 0.0f, 0.0f, 1.0f));
const mat4 result_a = glm::inverse(m * mirrorX);
Transform xform;
xform.setTranslation(t);
xform.setRotation(rot90);
xform.postScale(vec3(-1.0f, 1.0f, 1.0f));
mat4 result_b;
xform.getInverseMatrix(result_b);
// don't check elements directly, instead compare each axis transformed by the matrix.
auto xa = transformPoint(result_a, xAxis);
auto ya = transformPoint(result_a, yAxis);
auto za = transformPoint(result_a, zAxis);
auto xb = transformPoint(result_b, xAxis);
auto yb = transformPoint(result_b, yAxis);
auto zb = transformPoint(result_b, zAxis);
QCOMPARE_WITH_ABS_ERROR(xa, xb, TEST_EPSILON);
QCOMPARE_WITH_ABS_ERROR(ya, yb, TEST_EPSILON);
QCOMPARE_WITH_ABS_ERROR(za, zb, TEST_EPSILON);
}
void AnimInverseKinematicsTests::testBar() {
// test AnimPose math
// TODO: move this to other test file
glm::vec3 transA = glm::vec3(1.0f, 2.0f, 3.0f);
glm::vec3 transB = glm::vec3(5.0f, 7.0f, 9.0f);
glm::quat rot = identity;
glm::vec3 scale = glm::vec3(1.0f);
AnimPose poseA(scale, rot, transA);
AnimPose poseB(scale, rot, transB);
AnimPose poseC = poseA * poseB;
glm::vec3 expectedTransC = transA + transB;
QCOMPARE_WITH_ABS_ERROR(expectedTransC, poseC.trans, EPSILON);
}
void MeshMassPropertiesTests::testTetrahedron(){
// given the four vertices of a tetrahedron verify the analytic formula for inertia
// agrees with expected results
// these numbers from the Tonon paper:
btVector3 points[4];
points[0] = btVector3(8.33220f, -11.86875f, 0.93355f);
points[1] = btVector3(0.75523f, 5.00000f, 16.37072f);
points[2] = btVector3(52.61236f, 5.00000f, -5.38580f);
points[3] = btVector3(2.00000f, 5.00000f, 3.00000f);
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
// compute volume
btScalar volume = computeTetrahedronVolume(points);
btScalar error = (volume - expectedVolume) / expectedVolume;
if (fabsf(error) > acceptableRelativeError) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = "
<< error << std::endl;
}
btVector3 centerOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
// compute inertia tensor
// (shift the points so that tetrahedron's local centerOfMass is at origin)
for (int i = 0; i < 4; ++i) {
points[i] -= centerOfMass;
}
btMatrix3x3 inertia;
computeTetrahedronInertia(volume, points, inertia);
QCOMPARE_WITH_ABS_ERROR(volume, expectedVolume, acceptableRelativeError * volume);
QCOMPARE_WITH_RELATIVE_ERROR(inertia, expectedInertia, acceptableRelativeError);
}
void MovingMinMaxAvgTests::testInt() {
// int test
const int INTERVAL_LENGTH = 1;
const int WINDOW_INTERVALS = 75;
MovingMinMaxAvg<int> stats(INTERVAL_LENGTH, WINDOW_INTERVALS);
int min = std::numeric_limits<int>::max();
int max = 0;
double average = 0.0;
int totalSamples = 0;
int windowMin;
int windowMax;
double windowAverage;
QQueue<int> windowSamples;
// fill window samples
for (int i = 0; i < 100000; i++) {
int sample = rand();
windowSamples.enqueue(sample);
if (windowSamples.size() > INTERVAL_LENGTH * WINDOW_INTERVALS) {
windowSamples.dequeue();
}
stats.update(sample);
min = std::min(min, sample);
max = std::max(max, sample);
average = (average * totalSamples + sample) / (totalSamples + 1);
totalSamples++;
QCOMPARE(stats.getMin(), min);
QCOMPARE(stats.getMax(), max);
QCOMPARE_WITH_ABS_ERROR((float)stats.getAverage(), (float)average, EPSILON);
if ((i + 1) % INTERVAL_LENGTH == 0) {
QVERIFY(stats.getNewStatsAvailableFlag());
stats.clearNewStatsAvailableFlag();
windowMin = std::numeric_limits<int>::max();
windowMax = 0;
windowAverage = 0.0;
for (int s : windowSamples) {
windowMin = std::min(windowMin, s);
windowMax = std::max(windowMax, s);
windowAverage += (double)s;
}
windowAverage /= (double)windowSamples.size();
QCOMPARE(stats.getWindowMin(), windowMin);
QCOMPARE(stats.getWindowMax(), windowMax);
QCOMPARE_WITH_ABS_ERROR((float)stats.getAverage(), (float)average, EPSILON);
} else {
QVERIFY(!stats.getNewStatsAvailableFlag());
}
}
}
void AnimInverseKinematicsTests::testSingleChain() {
std::vector<FBXJoint> fbxJoints;
makeTestFBXJoints(fbxJoints);
// create a skeleton and doll
AnimPose offset;
AnimSkeleton* skeleton = new AnimSkeleton(fbxJoints, offset);
AnimSkeleton::Pointer skeletonPtr(skeleton);
AnimInverseKinematics ikDoll("doll");
ikDoll.setSkeleton(skeletonPtr);
{ // easy test IK of joint C
// load intial poses that look like this:
//
// A------>B------>C------>D
AnimPose pose;
pose.scale = glm::vec3(1.0f);
pose.rot = identity;
pose.trans = origin;
std::vector<AnimPose> poses;
poses.push_back(pose);
pose.trans = xAxis;
for (int i = 1; i < (int)fbxJoints.size(); ++i) {
poses.push_back(pose);
}
ikDoll.loadPoses(poses);
// we'll put a target t on D for <2, 1, 0> like this:
//
// t
//
//
// A------>B------>C------>D
//
glm::vec3 targetPosition(2.0f, 1.0f, 0.0f);
glm::quat targetRotation = glm::angleAxis(PI / 2.0f, zAxis);
AnimVariantMap varMap;
varMap.set("positionD", targetPosition);
varMap.set("rotationD", targetRotation);
ikDoll.setTargetVars("D", "positionD", "rotationD");
AnimNode::Triggers triggers;
// the IK solution should be:
//
// D
// |
// |
// A------>B------>C
//
float dt = 1.0f;
ikDoll.evaluate(varMap, dt, triggers);
// verify absolute results
// NOTE: since we expect this solution to converge very quickly (one loop)
// we can impose very tight error thresholds.
std::vector<AnimPose> absolutePoses;
ikDoll.computeAbsolutePoses(absolutePoses);
float acceptableAngle = 0.0001f;
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, EPSILON);
// verify relative results
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
}
{ // hard test IK of joint C
// load intial poses that look like this:
//
// D<------C
// |
// |
// A------>B
//
AnimPose pose;
pose.scale = glm::vec3(1.0f);
pose.rot = identity;
pose.trans = origin;
std::vector<AnimPose> poses;
poses.push_back(pose);
pose.trans = xAxis;
pose.rot = quaterTurnAroundZ;
poses.push_back(pose);
poses.push_back(pose);
poses.push_back(pose);
ikDoll.loadPoses(poses);
// we'll put a target t on D for <3, 0, 0> like this:
//
// D<------C
// |
// |
// A------>B t
//
glm::vec3 targetPosition(3.0f, 0.0f, 0.0f);
glm::quat targetRotation = identity;
AnimVariantMap varMap;
varMap.set("positionD", targetPosition);
varMap.set("rotationD", targetRotation);
ikDoll.setTargetVars("D", "positionD", "rotationD");
AnimNode::Triggers triggers;
// the IK solution should be:
//
// A------>B------>C------>D
//
float dt = 1.0f;
ikDoll.evaluate(varMap, dt, triggers);
// verify absolute results
// NOTE: the IK algorithm doesn't converge very fast for full-reach targets,
// so we'll consider the poses as good if they are closer than they started.
//
// NOTE: constraints may help speed up convergence since some joints may get clamped
// to maximum extension. TODO: experiment with tightening the error thresholds when
// constraints are working.
std::vector<AnimPose> absolutePoses;
ikDoll.computeAbsolutePoses(absolutePoses);
float acceptableAngle = 0.1f; // radians
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[2].rot, identity, acceptableAngle);
QCOMPARE_QUATS(absolutePoses[3].rot, identity, acceptableAngle);
float acceptableDistance = 0.4f;
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableDistance);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableDistance);
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, 3.0f * xAxis, acceptableDistance);
// verify relative results
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[2].rot, identity, acceptableAngle);
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
}
}
void GLMHelpersTests::testGenerateBasisVectors() {
{ // very simple case: primary along X, secondary is linear combination of X and Y
glm::vec3 u(1.0f, 0.0f, 0.0f);
glm::vec3 v(1.0f, 1.0f, 0.0f);
glm::vec3 w;
generateBasisVectors(u, v, u, v, w);
QCOMPARE_WITH_ABS_ERROR(u, Vectors::UNIT_X, EPSILON);
QCOMPARE_WITH_ABS_ERROR(v, Vectors::UNIT_Y, EPSILON);
QCOMPARE_WITH_ABS_ERROR(w, Vectors::UNIT_Z, EPSILON);
}
{ // point primary along Y instead of X
glm::vec3 u(0.0f, 1.0f, 0.0f);
glm::vec3 v(1.0f, 1.0f, 0.0f);
glm::vec3 w;
generateBasisVectors(u, v, u, v, w);
QCOMPARE_WITH_ABS_ERROR(u, Vectors::UNIT_Y, EPSILON);
QCOMPARE_WITH_ABS_ERROR(v, Vectors::UNIT_X, EPSILON);
QCOMPARE_WITH_ABS_ERROR(w, -Vectors::UNIT_Z, EPSILON);
}
{ // pass bad data (both vectors along Y). The helper will guess X for secondary.
glm::vec3 u(0.0f, 1.0f, 0.0f);
glm::vec3 v(0.0f, 1.0f, 0.0f);
glm::vec3 w;
generateBasisVectors(u, v, u, v, w);
QCOMPARE_WITH_ABS_ERROR(u, Vectors::UNIT_Y, EPSILON);
QCOMPARE_WITH_ABS_ERROR(v, Vectors::UNIT_X, EPSILON);
QCOMPARE_WITH_ABS_ERROR(w, -Vectors::UNIT_Z, EPSILON);
}
{ // pass bad data (both vectors along X). The helper will guess X for secondary, fail, then guess Y.
glm::vec3 u(1.0f, 0.0f, 0.0f);
glm::vec3 v(1.0f, 0.0f, 0.0f);
glm::vec3 w;
generateBasisVectors(u, v, u, v, w);
QCOMPARE_WITH_ABS_ERROR(u, Vectors::UNIT_X, EPSILON);
QCOMPARE_WITH_ABS_ERROR(v, Vectors::UNIT_Y, EPSILON);
QCOMPARE_WITH_ABS_ERROR(w, Vectors::UNIT_Z, EPSILON);
}
{ // general case for arbitrary rotation
float angle = 1.234f;
glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
glm::quat rotation = glm::angleAxis(angle, axis);
// expected values
glm::vec3 x = rotation * Vectors::UNIT_X;
glm::vec3 y = rotation * Vectors::UNIT_Y;
glm::vec3 z = rotation * Vectors::UNIT_Z;
// primary is along x
// secondary is linear combination of x and y
// tertiary is unknown
glm::vec3 u = 1.23f * x;
glm::vec3 v = 2.34f * x + 3.45f * y;
glm::vec3 w;
generateBasisVectors(u, v, u, v, w);
QCOMPARE_WITH_ABS_ERROR(u, x, EPSILON);
QCOMPARE_WITH_ABS_ERROR(v, y, EPSILON);
QCOMPARE_WITH_ABS_ERROR(w, z, EPSILON);
}
}
void MeshMassPropertiesTests::testBoxAsMesh() {
// verify that a mesh box produces the same mass properties as the analytic box.
// build a box:
// /
// y
// /
// 6-------------------------7
// /| /|
// / | / |
// / 2----------------------/--3
// / / / /
// | 4-------------------------5 / --x--
// z | / | /
// | |/ |/
// 0 ------------------------1
btScalar x(5.0f);
btScalar y(3.0f);
btScalar z(2.0f);
VectorOfPoints points;
points.reserve(8);
points.push_back(btVector3(0.0f, 0.0f, 0.0f));
points.push_back(btVector3(x, 0.0f, 0.0f));
points.push_back(btVector3(0.0f, y, 0.0f));
points.push_back(btVector3(x, y, 0.0f));
points.push_back(btVector3(0.0f, 0.0f, z));
points.push_back(btVector3(x, 0.0f, z));
points.push_back(btVector3(0.0f, y, z));
points.push_back(btVector3(x, y, z));
VectorOfIndices triangles;
pushTriangle(triangles, 0, 1, 4);
pushTriangle(triangles, 1, 5, 4);
pushTriangle(triangles, 1, 3, 5);
pushTriangle(triangles, 3, 7, 5);
pushTriangle(triangles, 2, 0, 6);
pushTriangle(triangles, 0, 4, 6);
pushTriangle(triangles, 3, 2, 7);
pushTriangle(triangles, 2, 6, 7);
pushTriangle(triangles, 4, 5, 6);
pushTriangle(triangles, 5, 7, 6);
pushTriangle(triangles, 0, 2, 1);
pushTriangle(triangles, 2, 3, 1);
// compute expected mass properties analytically
btVector3 expectedCenterOfMass = 0.5f * btVector3(x, y, z);
btScalar expectedVolume = x * y * z;
btMatrix3x3 expectedInertia;
computeBoxInertia(expectedVolume, btVector3(x, y, z), expectedInertia);
// compute the mass properties using the mesh
MeshMassProperties mesh(points, triangles);
// verify
QCOMPARE_WITH_ABS_ERROR(mesh._volume, expectedVolume, acceptableRelativeError * expectedVolume);
QCOMPARE_WITH_ABS_ERROR(mesh._centerOfMass, expectedCenterOfMass, acceptableAbsoluteError);
// test this twice: _RELATIVE_ERROR doesn't test zero cases (to avoid divide-by-zero); _ABS_ERROR does.
QCOMPARE_WITH_ABS_ERROR(mesh._inertia, expectedInertia, acceptableAbsoluteError);
QCOMPARE_WITH_RELATIVE_ERROR(mesh._inertia, expectedInertia, acceptableRelativeError);
// These two macros impl this:
// for (int i = 0; i < 3; ++i) {
// for (int j = 0; j < 3; ++j) {
// if (expectedInertia [i][j] == btScalar(0.0f)) {
// error = mesh._inertia[i][j] - expectedInertia[i][j]; // COMPARE_WITH_ABS_ERROR
// if (fabsf(error) > acceptableAbsoluteError) {
// std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by "
// << error << " absolute"<< std::endl;
// }
// } else {
// error = (mesh._inertia[i][j] - expectedInertia[i][j]) / expectedInertia[i][j]; // COMPARE_WITH_RELATIVE_ERROR
// if (fabsf(error) > acceptableRelativeError) {
// std::cout << __FILE__ << ":" << __LINE__ << " ERROR : inertia[" << i << "][" << j << "] off by "
// << error << std::endl;
// }
// }
// }
// }
}
void RotationConstraintTests::testElbowConstraint() {
// referenceRotation is the default rotation
float referenceAngle = 1.23f;
glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f));
glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis);
// NOTE: hingeAxis is in the "referenceFrame"
glm::vec3 hingeAxis = glm::vec3(1.0f, 0.0f, 0.0f);
// the angle limits of the constriant about the hinge axis
float minAngle = -PI / 4.0f;
float maxAngle = PI / 3.0f;
// build the constraint
ElbowConstraint elbow;
elbow.setReferenceRotation(referenceRotation);
elbow.setHingeAxis(hingeAxis);
elbow.setAngleLimits(minAngle, maxAngle);
float smallAngle = PI / 100.0f;
{ // test reference rotation -- should be unconstrained
glm::quat inputRotation = referenceRotation;
glm::quat outputRotation = inputRotation;
bool updated = elbow.apply(outputRotation);
QVERIFY(updated == false);
glm::quat expectedRotation = referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
{ // test several simple rotations that are INSIDE the limits -- should be unconstrained
int numChecks = 10;
float startAngle = minAngle + smallAngle;
float endAngle = maxAngle - smallAngle;
float deltaAngle = (endAngle - startAngle) / (float)(numChecks - 1);
for (float angle = startAngle; angle < endAngle + 0.5f * deltaAngle; angle += deltaAngle) {
glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
glm::quat outputRotation = inputRotation;
bool updated = elbow.apply(outputRotation);
QVERIFY(updated == false);
QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON);
}
}
{ // test simple rotation just OUTSIDE minAngle -- should be constrained
float angle = minAngle - smallAngle;
glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
glm::quat outputRotation = inputRotation;
bool updated = elbow.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedRotation = glm::angleAxis(minAngle, hingeAxis) * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
{ // test simple rotation just OUTSIDE maxAngle -- should be constrained
float angle = maxAngle + smallAngle;
glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
glm::quat outputRotation = inputRotation;
bool updated = elbow.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedRotation = glm::angleAxis(maxAngle, hingeAxis) * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
{ // test simple twist rotation that has no hinge component -- should be constrained
glm::vec3 someVector(7.0f, -5.0f, 2.0f);
glm::vec3 twistVector = glm::normalize(glm::cross(hingeAxis, someVector));
float someAngle = 0.789f;
glm::quat inputRotation = glm::angleAxis(someAngle, twistVector) * referenceRotation;
glm::quat outputRotation = inputRotation;
bool updated = elbow.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedRotation = referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
}
void RotationConstraintTests::testSwingTwistConstraint() {
// referenceRotation is the default rotation
float referenceAngle = 1.23f;
glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f));
glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis);
// the angle limits of the constriant about the hinge axis
float minTwistAngle = -PI / 2.0f;
float maxTwistAngle = PI / 2.0f;
// build the constraint
SwingTwistConstraint shoulder;
shoulder.setReferenceRotation(referenceRotation);
shoulder.setTwistLimits(minTwistAngle, maxTwistAngle);
float lowDot = 0.25f;
float highDot = 0.75f;
// The swing constriants are more interesting: a vector of minimum dot products
// as a function of theta around the twist axis. Our test function will be shaped
// like the square wave with amplitudes 0.25 and 0.75:
//
// |
// 0.75 - o---o---o---o
// | / '
// | / '
// | / '
// 0.25 o---o---o---o o
// |
// +-------+-------+-------+-------+---
// 0 pi/2 pi 3pi/2 2pi
int numDots = 8;
std::vector<float> minDots;
int dotIndex = 0;
while (dotIndex < numDots / 2) {
++dotIndex;
minDots.push_back(lowDot);
}
while (dotIndex < numDots) {
minDots.push_back(highDot);
++dotIndex;
}
shoulder.setSwingLimits(minDots);
const SwingTwistConstraint::SwingLimitFunction& shoulderSwingLimitFunction = shoulder.getSwingLimitFunction();
{ // test interpolation of SwingLimitFunction
const float ACCEPTABLE_ERROR = 1.0e-5f;
float theta = 0.0f;
float minDot = shoulderSwingLimitFunction.getMinDot(theta);
float expectedMinDot = lowDot;
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
theta = PI;
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = highDot;
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
// test interpolation on upward slope
theta = PI * (7.0f / 8.0f);
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = 0.5f * (highDot + lowDot);
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
// test interpolation on downward slope
theta = PI * (15.0f / 8.0f);
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = 0.5f * (highDot + lowDot);
}
float smallAngle = PI / 100.0f;
// Note: the twist is always about the yAxis
glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
{ // test INSIDE both twist and swing
int numSwingAxes = 7;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle + smallAngle;
float endTwist = maxTwistAngle - smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float swing = acosf(shoulderSwingLimitFunction.getMinDot(theta)) - smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == false);
QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test INSIDE twist but OUTSIDE swing
int numSwingAxes = 7;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle + smallAngle;
float endTwist = maxTwistAngle - smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle + smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
glm::quat expectedRotation = expectedSwingRotation * twistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test OUTSIDE twist but INSIDE swing
int numSwingAxes = 6;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle - smallAngle;
float endTwist = maxTwistAngle + smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle - smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
glm::quat expectedRotation = swingRotation * expectedTwistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test OUTSIDE both twist and swing
int numSwingAxes = 5;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle - smallAngle;
float endTwist = maxTwistAngle + smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle + smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
glm::quat expectedRotation = expectedSwingRotation * expectedTwistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
}
