Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-12 07:18:21 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

add AngularConstraint.* with some unit tests

Browse source 
more unit tests to follow
This commit is contained in:
Andrew Meadows 2014-06-30 18:05:22 -07:00
parent 9eba4b01f0
commit 9ca1bfdfe5
6 changed files with 553 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
interface/src/renderer/JointState.cpp
View file

@ -11,6 +11,7 @@
#include <glm/gtx/norm.hpp>
#include <AngularConstraint.h>
//#include <GeometryUtil.h>
#include <SharedUtil.h>
@ -18,7 +19,13 @@
JointState::JointState() :
_animationPriority(0.0f),
_fbxJoint(NULL) {
_fbxJoint(NULL),
_constraint(NULL) {
}
JointState::~JointState() {
delete _constraint;
_constraint = NULL;
}
void JointState::setFBXJoint(const FBXJoint* joint) {

4
interface/src/renderer/JointState.h
View file

@ -18,9 +18,12 @@
#include <FBXReader.h>
class AngularConstraint;
class JointState {
public:
JointState();
~JointState();
void setFBXJoint(const FBXJoint* joint);
const FBXJoint& getFBXJoint() const { return *_fbxJoint; }
@ -61,6 +64,7 @@ private:
glm::quat _rotation; // joint- to model-frame
const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint
AngularConstraint* _constraint;
};
#endif // hifi_JointState_h

176
libraries/shared/src/AngularConstraint.cpp Normal file
View file

@ -0,0 +1,176 @@
//
// AngularConstraint.cpp
// interface/src/renderer
//
// Created by Andrew Meadows on 2014.05.30
// Copyright 2014 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include <iostream> // adebug
#include <glm/gtx/norm.hpp>
#include "AngularConstraint.h"
#include "SharedUtil.h"
#include "StreamUtils.h" // adebug
// helper function
/// \param angle radian angle to be clamped within angleMin and angleMax
/// \param angleMin minimum value
/// \param angleMax maximum value
/// \return value between minAngle and maxAngle closest to angle
float clampAngle(float angle, float angleMin, float angleMax) {
float minDistance = angle - angleMin;
float maxDistance = angle - angleMax;
if (maxDistance > 0.0f) {
minDistance = glm::min(minDistance, angleMin + TWO_PI - angle);
angle = (minDistance < maxDistance) ? angleMin : angleMax;
} else if (minDistance < 0.0f) {
maxDistance = glm::max(maxDistance, angleMax - TWO_PI - angle);
angle = (minDistance > maxDistance) ? angleMin : angleMax;
}
return angle;
}
// static
AngularConstraint* AngularConstraint::newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles) {
float minDistance2 = glm::distance2(minAngles, glm::vec3(-PI, -PI, -PI));
float maxDistance2 = glm::distance2(maxAngles, glm::vec3(PI, PI, PI));
if (minDistance2 < EPSILON && maxDistance2 < EPSILON) {
// no constraint
return NULL;
}
// count the zero length elements
glm::vec3 rangeAngles = maxAngles - minAngles;
int pivotIndex = -1;
int numZeroes = 0;
for (int i = 0; i < 3; ++i) {
if (rangeAngles[i] < EPSILON) {
++numZeroes;
} else {
pivotIndex = i;
}
}
if (numZeroes == 2) {
// this is a hinge
int forwardIndex = (pivotIndex + 1) % 3;
glm::vec3 forwardAxis(0.0f);
forwardAxis[forwardIndex] = 1.0f;
glm::vec3 rotationAxis(0.0f);
rotationAxis[pivotIndex] = 1.0f;
return new HingeConstraint(forwardAxis, rotationAxis, minAngles[pivotIndex], maxAngles[pivotIndex]);
} else if (numZeroes == 0) {
// approximate the angular limits with a cone roller
// we assume the roll is about z
glm::vec3 middleAngles = 0.5f * (maxAngles - minAngles);
glm::quat yaw = glm::angleAxis(middleAngles[1], glm::vec3(0.0f, 1.0f, 0.0f));
glm::quat pitch = glm::angleAxis(middleAngles[0], glm::vec3(1.0f, 0.0f, 0.0f));
glm::vec3 coneAxis = pitch * yaw * glm::vec3(0.0f, 0.0f, 1.0f);
// the coneAngle is half of the average range of the two non-roll rotations
float coneAngle = 0.25f * (middleAngles[0] + middleAngles[1]);
return new ConeRollerConstraint(coneAngle, coneAxis, minAngles.z, maxAngles.z);
}
return NULL;
}
HingeConstraint::HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle)
: _minAngle(minAngle), _maxAngle(maxAngle) {
assert(_minAngle < _maxAngle);
// we accept the rotationAxis direction
assert(glm::length(rotationAxis) > EPSILON);
_rotationAxis = glm::normalize(rotationAxis);
// but we compute the final _forwardAxis
glm::vec3 otherAxis = glm::cross(_rotationAxis, forwardAxis);
assert(glm::length(otherAxis) > EPSILON);
_forwardAxis = glm::normalize(glm::cross(otherAxis, _rotationAxis));
}
// virtual
bool HingeConstraint::applyTo(glm::quat& rotation) const {
glm::vec3 forward = rotation * _forwardAxis;
forward -= glm::dot(forward, _rotationAxis) * _rotationAxis;
float length = glm::length(forward);
if (length < EPSILON) {
// infinite number of solutions ==> choose the middle of the contrained range
rotation = glm::angleAxis(0.5f * (_minAngle + _maxAngle), _rotationAxis);
return true;
}
forward /= length;
float sign = (glm::dot(glm::cross(_forwardAxis, forward), _rotationAxis) > 0.0f ? 1.0f : -1.0f);
//float angle = sign * acos(glm::dot(forward, _forwardAxis) / length);
float angle = sign * acos(glm::dot(forward, _forwardAxis));
// std::cout << "adebug forward = " << forward << " _forwardAxis = " << _forwardAxis << " angle = " << angle << std::endl; // adebug
glm::quat newRotation = glm::angleAxis(clampAngle(angle, _minAngle, _maxAngle), _rotationAxis);
if (fabsf(1.0f - glm::dot(newRotation, rotation)) > EPSILON * EPSILON) {
rotation = newRotation;
return true;
}
return false;
}
ConeRollerConstraint::ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll)
: _coneAngle(coneAngle), _minRoll(minRoll), _maxRoll(maxRoll) {
assert(_maxRoll >= _minRoll);
float axisLength = glm::length(coneAxis);
assert(axisLength > EPSILON);
_coneAxis = coneAxis / axisLength;
}
// virtual
bool ConeRollerConstraint::applyTo(glm::quat& rotation) const {
bool applied = false;
glm::vec3 rotatedAxis = rotation * _coneAxis;
glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis);
float perpAxisLength = glm::length(perpAxis);
// enforce the cone
float angle = acosf(glm::dot(rotatedAxis, _coneAxis));
if (angle > _coneAngle && perpAxisLength > EPSILON) {
perpAxis /= perpAxisLength;
rotation = glm::angleAxis(angle - _coneAngle, perpAxis) * rotation;
rotatedAxis = rotation * _coneAxis;
applied = true;
}
// enforce the roll
if (perpAxisLength < EPSILON) {
// there is no obvious perp axis so we must pick one
perpAxis = rotatedAxis;
// find the first non-zero element:
float value = 0.0f;
int i = 0;
for (i = 0; i < 3; ++i) {
if (fabsf(perpAxis[i]) > EPSILON) {
value = perpAxis[i];
break;
}
}
assert(i != 3);
// swap or negate the next element
int j = (i + 1) % 3;
float value2 = perpAxis[j];
if (fabsf(value2 - value) > EPSILON) {
perpAxis[i] = value2;
perpAxis[j] = value;
} else {
perpAxis[i] = -value;
}
perpAxis = glm::cross(perpAxis, rotatedAxis);
perpAxisLength = glm::length(perpAxis);
assert(perpAxisLength > EPSILON);
}
perpAxis /= perpAxisLength;
glm::vec3 rotatedPerpAxis = rotation * perpAxis;
float roll = angleBetween(rotatedPerpAxis, perpAxis);
if (roll < _minRoll || roll > _maxRoll) {
float newRoll = clampAngle(roll, _minRoll, _maxRoll);
rotation = glm::angleAxis(newRoll - roll, rotatedAxis) * rotation;
applied = true;
}
return applied;
}

53
libraries/shared/src/AngularConstraint.h Normal file
View file

@ -0,0 +1,53 @@
//
// AngularConstraint.h
// interface/src/renderer
//
// Created by Andrew Meadows on 2014.05.30
// Copyright 2013 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_AngularConstraint_h
#define hifi_AngularConstraint_h
#include <glm/glm.hpp>
class AngularConstraint {
public:
/// \param minAngles minumum euler angles for the constraint
/// \param maxAngles minumum euler angles for the constraint
/// \return pointer to new AngularConstraint of the right type or NULL if none could be made
static AngularConstraint* newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles);
AngularConstraint() {}
virtual ~AngularConstraint() {}
virtual bool applyTo(glm::quat& rotation) const = 0;
protected:
};
class HingeConstraint : public AngularConstraint {
public:
HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle);
virtual bool applyTo(glm::quat& rotation) const;
protected:
glm::vec3 _forwardAxis;
glm::vec3 _rotationAxis;
float _minAngle;
float _maxAngle;
};
class ConeRollerConstraint : public AngularConstraint {
public:
ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll);
virtual bool applyTo(glm::quat& rotation) const;
private:
float _coneAngle;
glm::vec3 _coneAxis;
float _minRoll;
float _maxRoll;
};
#endif // hifi_AngularConstraint_h

291
tests/shared/src/AngularConstraintTests.cpp Normal file
View file

@ -0,0 +1,291 @@
//
// AngularConstraintTests.cpp
// tests/physics/src
//
// Created by Andrew Meadows on 2014.05.30
// Copyright 2014 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include <iostream>
#include <AngularConstraint.h>
#include <SharedUtil.h>
#include <StreamUtils.h>
#include "AngularConstraintTests.h"
void AngularConstraintTests::testHingeConstraint() {
float minAngle = -PI;
float maxAngle = 0.0f;
glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
glm::vec3 minAngles(0.0f, -PI, 0.0f);
glm::vec3 maxAngles(0.0f, 0.0f, 0.0f);
AngularConstraint* c = AngularConstraint::newAngularConstraint(minAngles, maxAngles);
if (!c) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: newAngularConstraint() should make a constraint" << std::endl;
}
{ // test in middle of constraint
float angle = 0.5f * (minAngle + maxAngle);
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not applyTo()" << std::endl;
}
if (rotation != newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not change rotation" << std::endl;
}
}
{ // test just inside min edge of constraint
float angle = minAngle + 10.f * EPSILON;
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not applyTo()" << std::endl;
}
if (rotation != newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not change rotation" << std::endl;
}
}
{ // test just inside max edge of constraint
float angle = maxAngle - 10.f * EPSILON;
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not applyTo()" << std::endl;
}
if (rotation != newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should not change rotation" << std::endl;
}
}
{ // test just outside min edge of constraint
float angle = minAngle - 0.001f;
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test just outside max edge of constraint
float angle = maxAngle + 0.001f;
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test far outside min edge of constraint (wraps around to max)
float angle = minAngle - 0.75f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test far outside max edge of constraint (wraps around to min)
float angle = maxAngle + 0.75f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
float ACCEPTABLE_ERROR = 1.0e-4f;
{ // test nearby but off-axis rotation
float offAngle = 0.1f;
glm::quat offRotation(offAngle, glm::vec3(1.0f, 0.0f, 0.0f));
float angle = 0.5f * (maxAngle + minAngle);
glm::quat rotation = offRotation * glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(angle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test way off rotation > maxAngle
float offAngle = 0.5f;
glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f));
float angle = maxAngle + 0.2f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
rotation = offRotation * glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test way off rotation < minAngle
float offAngle = 0.5f;
glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f));
float angle = minAngle - 0.2f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
rotation = offRotation * glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test way off rotation > maxAngle with wrap over to minAngle
float offAngle = -0.5f;
glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f));
float angle = maxAngle + 0.6f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
rotation = offRotation * glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(minAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
{ // test way off rotation < minAngle with wrap over to maxAngle
float offAngle = -0.6f;
glm::quat offRotation = glm::angleAxis(offAngle, glm::vec3(1.0f, 0.0f, 0.0f));
float angle = minAngle - 0.7f * (TWO_PI - (maxAngle - minAngle));
glm::quat rotation = glm::angleAxis(angle, yAxis);
rotation = offRotation * glm::angleAxis(angle, yAxis);
glm::quat newRotation = rotation;
bool constrained = c->applyTo(newRotation);
if (!constrained) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should applyTo()" << std::endl;
}
if (rotation == newRotation) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint should change rotation" << std::endl;
}
glm::quat expectedRotation = glm::angleAxis(maxAngle, yAxis);
float qDot = glm::dot(expectedRotation, newRotation);
if (fabsf(qDot - 1.0f) > ACCEPTABLE_ERROR) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: HingeConstraint rotation = " << newRotation << " but expected " << expectedRotation << std::endl;
}
}
}
void AngularConstraintTests::testConeRollerConstraint() {
}
void AngularConstraintTests::runAllTests() {
testHingeConstraint();
}

21
tests/shared/src/AngularConstraintTests.h Normal file
View file

@ -0,0 +1,21 @@
//
// AngularConstraintTests.h
// tests/physics/src
//
// Created by Andrew Meadows on 2014.05.30
// Copyright 2014 High Fidelity, Inc.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_AngularConstraintTests_h
#define hifi_AngularConstraintTests_h
namespace AngularConstraintTests {
void testHingeConstraint();
void testConeRollerConstraint();
void runAllTests();
}
#endif // hifi_AngularConstraintTests_h