Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-16 11:52:04 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #5545 from AndrewMeadows/chlorine

Browse source 
rotation constraints in preparation for inverse kinematics
This commit is contained in:
Howard Stearns 2015-08-11 09:19:16 -07:00
commit 295d655eec
14 changed files with 864 additions and 2059 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

80
libraries/animation/src/ElbowConstraint.cpp Normal file
View file

@ -0,0 +1,80 @@
//
// ElbowConstraint.cpp
//
// Copyright 2015 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 "ElbowConstraint.h"
#include <algorithm>
#include <GeometryUtil.h>
#include <NumericalConstants.h>
ElbowConstraint::ElbowConstraint() :
_referenceRotation(),
_minAngle(-PI),
_maxAngle(PI)
{
}
void ElbowConstraint::setHingeAxis(const glm::vec3& axis) {
float axisLength = glm::length(axis);
assert(axisLength > EPSILON);
_axis = axis / axisLength;
// for later we need a second axis that is perpendicular to the first
for (int i = 0; i < 3; ++i) {
float component = _axis[i];
const float MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT = 0.57735f; // just under 1/sqrt(3)
if (fabsf(component) > MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT) {
int j = (i + 1) % 3;
int k = (j + 1) % 3;
_perpAxis[i] = - _axis[j];
_perpAxis[j] = component;
_perpAxis[k] = 0.0f;
_perpAxis = glm::normalize(_perpAxis);
break;
}
}
}
void ElbowConstraint::setAngleLimits(float minAngle, float maxAngle) {
// NOTE: min/maxAngle angles should be in the range [-PI, PI]
_minAngle = glm::min(minAngle, maxAngle);
_maxAngle = glm::max(minAngle, maxAngle);
}
bool ElbowConstraint::apply(glm::quat& rotation) const {
// decompose the rotation into swing and twist
// NOTE: rotation = postRotation * referenceRotation
glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
glm::quat swingRotation, twistRotation;
swingTwistDecomposition(postRotation, _axis, swingRotation, twistRotation);
// NOTE: postRotation = swingRotation * twistRotation
// compute twistAngle
float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
glm::vec3 twisted = twistRotation * _perpAxis;
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(_perpAxis, twisted), _axis));
// clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minAngle, _maxAngle);
bool twistWasClamped = (twistAngle != clampedTwistAngle);
// update rotation
const float MIN_SWING_REAL_PART = 0.99999f;
if (twistWasClamped || fabsf(swingRotation.w < MIN_SWING_REAL_PART)) {
if (twistWasClamped) {
twistRotation = glm::angleAxis(clampedTwistAngle, _axis);
}
// we discard all swing and only keep twist
rotation = twistRotation * _referenceRotation;
return true;
}
return false;
}

30
libraries/animation/src/ElbowConstraint.h Normal file
View file

@ -0,0 +1,30 @@
//
// ElbowConstraint.h
//
// Copyright 2015 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_ElbowConstraint_h
#define hifi_ElbowConstraint_h
#include "RotationConstraint.h"
class ElbowConstraint : public RotationConstraint {
public:
ElbowConstraint();
virtual void setReferenceRotation(const glm::quat& rotation) override { _referenceRotation = rotation; }
void setHingeAxis(const glm::vec3& axis);
void setAngleLimits(float minAngle, float maxAngle);
virtual bool apply(glm::quat& rotation) const override;
protected:
glm::quat _referenceRotation;
glm::vec3 _axis;
glm::vec3 _perpAxis;
float _minAngle;
float _maxAngle;
};
#endif // hifi_ElbowConstraint_h

29
libraries/animation/src/RotationConstraint.h Normal file
View file

@ -0,0 +1,29 @@
//
// RotationConstraint.h
//
// Copyright 2015 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_RotationConstraint_h
#define hifi_RotationConstraint_h
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
class RotationConstraint {
public:
RotationConstraint() {}
virtual ~RotationConstraint() {}
/// \param rotation the default rotation that represents
virtual void setReferenceRotation(const glm::quat& rotation) = 0;
/// \param rotation rotation to clamp
/// \return true if rotation is clamped
virtual bool apply(glm::quat& rotation) const = 0;
};
#endif // hifi_RotationConstraint_h

128
libraries/animation/src/SwingTwistConstraint.cpp Normal file
View file

@ -0,0 +1,128 @@
//
// SwingTwistConstraint.cpp
//
// Copyright 2015 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 "SwingTwistConstraint.h"
#include <algorithm>
#include <math.h>
#include <GeometryUtil.h>
#include <NumericalConstants.h>
const float MIN_MINDOT = -0.999f;
const float MAX_MINDOT = 1.0f;
SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {
setCone(PI);
}
void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) {
_minDots.clear();
float minDot = glm::clamp(maxAngle, MIN_MINDOT, MAX_MINDOT);
_minDots.push_back(minDot);
// push the first value to the back to establish cyclic boundary conditions
_minDots.push_back(minDot);
}
void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {
int numDots = minDots.size();
_minDots.clear();
_minDots.reserve(numDots);
for (int i = 0; i < numDots; ++i) {
_minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));
}
// push the first value to the back to establish cyclic boundary conditions
_minDots.push_back(_minDots[0]);
}
float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
// extract the positive normalized fractional part of theta
float integerPart;
float normalizedTheta = modff(theta / TWO_PI, &integerPart);
if (normalizedTheta < 0.0f) {
normalizedTheta += 1.0f;
}
// interpolate between the two nearest points in the cycle
float fractionPart = modff(normalizedTheta * (float)(_minDots.size() - 1), &integerPart);
int i = (int)(integerPart);
int j = (i + 1) % _minDots.size();
return _minDots[i] * (1.0f - fractionPart) + _minDots[j] * fractionPart;
}
SwingTwistConstraint::SwingTwistConstraint() :
_swingLimitFunction(),
_referenceRotation(),
_minTwist(-PI),
_maxTwist(PI)
{
}
void SwingTwistConstraint::setSwingLimits(std::vector<float> minDots) {
_swingLimitFunction.setMinDots(minDots);
}
void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
// NOTE: min/maxTwist angles should be in the range [-PI, PI]
_minTwist = glm::min(minTwist, maxTwist);
_maxTwist = glm::max(minTwist, maxTwist);
}
bool SwingTwistConstraint::apply(glm::quat& rotation) const {
// decompose the rotation into first twist about yAxis, then swing about something perp
const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
// NOTE: rotation = postRotation * referenceRotation
glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
glm::quat swingRotation, twistRotation;
swingTwistDecomposition(postRotation, yAxis, swingRotation, twistRotation);
// NOTE: postRotation = swingRotation * twistRotation
// compute twistAngle
float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
const glm::vec3 xAxis = glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 twistedX = twistRotation * xAxis;
twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis));
// clamp twistAngle
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
bool twistWasClamped = (twistAngle != clampedTwistAngle);
// clamp the swing
// The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
glm::vec3 swungY = swingRotation * yAxis;
glm::vec3 swingAxis = glm::cross(yAxis, swungY);
bool swingWasClamped = false;
float axisLength = glm::length(swingAxis);
if (axisLength > EPSILON) {
// The limit of swing is a function of "theta" which can be computed from the swingAxis
// (which is in the constraint's ZX plane).
float theta = atan2f(-swingAxis.z, swingAxis.x);
float minDot = _swingLimitFunction.getMinDot(theta);
if (glm::dot(swungY, yAxis) < minDot) {
// The swing limits are violated so we use the maxAngle to supply a new rotation.
float maxAngle = acosf(minDot);
if (minDot < 0.0f) {
maxAngle = PI - maxAngle;
}
swingAxis /= axisLength;
swingRotation = glm::angleAxis(maxAngle, swingAxis);
swingWasClamped = true;
}
}
if (swingWasClamped || twistWasClamped) {
twistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
rotation = swingRotation * twistRotation * _referenceRotation;
return true;
}
return false;
}

79
libraries/animation/src/SwingTwistConstraint.h Normal file
View file

@ -0,0 +1,79 @@
//
// SwingTwistConstraint.h
//
// Copyright 2015 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_SwingTwistConstraint_h
#define hifi_SwingTwistConstraint_h
#include <vector>
#include "RotationConstraint.h"
#include <math.h>
class SwingTwistConstraint : RotationConstraint {
public:
// The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist
// about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist
// and swing combine to produce the rotation. Each partial rotation is constrained within limits
// then used to construct the new final rotation.
SwingTwistConstraint();
/// \param referenceRotation the rotation from which rotation changes are measured.
virtual void setReferenceRotation(const glm::quat& referenceRotation) override { _referenceRotation = referenceRotation; }
/// \param minDots vector of minimum dot products between the twist and swung axes.
/// \brief The values are minimum dot-products between the twist axis and the swung axes
/// that correspond to swing axes equally spaced around the XZ plane. Another way to
/// think about it is that the dot-products correspond to correspond to angles (theta)
/// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary
/// conditions are handled internally, so don't duplicate the dot-product at 2PI).
/// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed
/// description of how this works.
void setSwingLimits(std::vector<float> minDots);
/// \param minTwist the minimum angle of rotation about the twist axis
/// \param maxTwist the maximum angle of rotation about the twist axis
void setTwistLimits(float minTwist, float maxTwist);
/// \param rotation[in/out] the current rotation to be modified to fit within constraints
/// \return true if rotation is changed
virtual bool apply(glm::quat& rotation) const override;
// SwingLimitFunction is an implementation of the constraint check described in the paper:
// "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash
class SwingLimitFunction {
public:
SwingLimitFunction();
/// \brief use a uniform conical swing limit
void setCone(float maxAngle);
/// \brief use a vector of lookup values for swing limits
void setMinDots(const std::vector<float>& minDots);
/// \return minimum dotProduct between reference and swung axes
float getMinDot(float theta) const;
protected:
// the limits are stored in a lookup table with cyclic boundary conditions
std::vector<float> _minDots;
};
/// \return reference to SwingLimitFunction instance for unit-testing
const SwingLimitFunction& getSwingLimitFunction() const { return _swingLimitFunction; }
protected:
SwingLimitFunction _swingLimitFunction;
glm::quat _referenceRotation;
float _minTwist;
float _maxTwist;
};
#endif // hifi_SwingTwistConstraint_h

98
libraries/shared/src/GeometryUtil.cpp
View file

@ -9,11 +9,13 @@
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "GeometryUtil.h"
#include <assert.h>
#include <cstring>
#include <cmath>
#include <glm/gtx/quaternion.hpp>
#include "GeometryUtil.h"
#include "NumericalConstants.h"
glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {
@ -26,20 +28,20 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
float proj = glm::dot(point - start, segmentVector) / lengthSquared;
if (proj <= 0.0f) { // closest to the start
return start - point;
} else if (proj >= 1.0f) { // closest to the end
return end - point;
} else { // closest to the middle
return start + segmentVector*proj - point;
}
}
// Computes the penetration between a point and a sphere (centered at the origin)
// if point is inside sphere: returns true and stores the result in 'penetration'
// if point is inside sphere: returns true and stores the result in 'penetration'
// (the vector that would move the point outside the sphere)
// otherwise returns false
bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius,
bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius,
glm::vec3& penetration) {
float vectorLength = glm::length(point);
if (vectorLength < EPSILON) {
@ -71,7 +73,7 @@ bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius
bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration) {
return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd),
return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd),
glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);
}
@ -93,10 +95,10 @@ bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& ca
float proj = glm::dot(point - capsuleStart, segmentVector) / lengthSquared;
if (proj <= 0.0f) { // closest to the start
return findPointSpherePenetration(point, capsuleStart, startRadius, penetration);
} else if (proj >= 1.0f) { // closest to the end
return findPointSpherePenetration(point, capsuleEnd, endRadius, penetration);
} else { // closest to the middle
return findPointSpherePenetration(point, capsuleStart + segmentVector * proj,
glm::mix(startRadius, endRadius, proj), penetration);
@ -110,7 +112,7 @@ bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter,
startRadius + sphereRadius, endRadius + sphereRadius, penetration);
}
bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec4& plane, glm::vec3& penetration) {
float distance = glm::dot(plane, glm::vec4(sphereCenter, 1.0f)) - sphereRadius;
if (distance < 0.0f) {
@ -120,8 +122,8 @@ bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadiu
return false;
}
bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
glm::vec3& penetration) {
glm::vec3 localCenter = sphereCenter - diskCenter;
float axialDistance = glm::dot(localCenter, diskNormal);
@ -171,12 +173,12 @@ glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3&
}
glm::vec3 currentDirection = currentPenetration / currentLength;
float directionalComponent = glm::dot(newPenetration, currentDirection);
// if orthogonal or in the opposite direction, we can simply add
if (directionalComponent <= 0.0f) {
return currentPenetration + newPenetration;
}
// otherwise, we need to take the maximum component of current and new
return currentDirection * glm::max(directionalComponent, currentLength) +
newPenetration - (currentDirection * directionalComponent);
@ -217,14 +219,14 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
float c = glm::dot(constant, constant) - radius * radius;
if (c < 0.0f) { // starts inside cylinder
if (originProjection < 0.0f) { // below start
return findRaySphereIntersection(origin, direction, start, radius, distance);
return findRaySphereIntersection(origin, direction, start, radius, distance);
} else if (originProjection > capsuleLength) { // above end
return findRaySphereIntersection(origin, direction, end, radius, distance);
return findRaySphereIntersection(origin, direction, end, radius, distance);
} else { // between start and end
distance = 0.0f;
return true;
return true;
}
}
glm::vec3 coefficient = direction - relativeEnd * glm::dot(relativeEnd, direction);
@ -245,10 +247,10 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
float intersectionProjection = glm::dot(relativeEnd, intersection);
if (intersectionProjection < 0.0f) { // below start
return findRaySphereIntersection(origin, direction, start, radius, distance);
} else if (intersectionProjection > capsuleLength) { // above end
return findRaySphereIntersection(origin, direction, end, radius, distance);
}
}
distance = t; // between start and end
return true;
}
@ -311,7 +313,7 @@ int computeDirection(float xi, float yi, float xj, float yj, float xk, float yk)
//
// (0,0) (windowWidth, 0)
// -1,1 1,1
// +-----------------------+
// +-----------------------+
// | | |
// | | |
// | -1,0 | |
@ -341,10 +343,10 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
int maxLength = inLength * 2;
glm::vec2* tempVertexArrayA = new glm::vec2[maxLength];
glm::vec2* tempVertexArrayB = new glm::vec2[maxLength];
// set up our temporary arrays
memcpy(tempVertexArrayA, inputVertexArray, sizeof(glm::vec2) * inLength);
// Left edge
LineSegment2 edge;
edge[0] = TOP_LEFT_CLIPPING_WINDOW;
@ -353,7 +355,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
// clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
// Bottom Edge
edge[0] = BOTTOM_LEFT_CLIPPING_WINDOW;
edge[1] = BOTTOM_RIGHT_CLIPPING_WINDOW;
@ -361,7 +363,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
// clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
// Right Edge
edge[0] = BOTTOM_RIGHT_CLIPPING_WINDOW;
edge[1] = TOP_RIGHT_CLIPPING_WINDOW;
@ -369,7 +371,7 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
// clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
// Top Edge
edge[0] = TOP_RIGHT_CLIPPING_WINDOW;
edge[1] = TOP_LEFT_CLIPPING_WINDOW;
@ -377,18 +379,18 @@ void PolygonClip::clipToScreen(const glm::vec2* inputVertexArray, int inLength,
sutherlandHodgmanPolygonClip(tempVertexArrayA, tempVertexArrayB, tempLengthA, tempLengthB, edge);
// clean the array from tempVertexArrayA and copy cleaned result to tempVertexArrayA
copyCleanArray(tempLengthA, tempVertexArrayA, tempLengthB, tempVertexArrayB);
// copy final output to outputVertexArray
outputVertexArray = tempVertexArrayA;
outLength = tempLengthA;
// cleanup our unused temporary buffer...
delete[] tempVertexArrayB;
// Note: we don't delete tempVertexArrayA, because that's the caller's responsibility
}
void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
int inLength, int& outLength, const LineSegment2& clipBoundary) {
glm::vec2 start, end; // Start, end point of current polygon edge
glm::vec2 intersection; // Intersection point with a clip boundary
@ -397,8 +399,8 @@ void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::ve
start = inVertexArray[inLength - 1]; // Start with the last vertex in inVertexArray
for (int j = 0; j < inLength; j++) {
end = inVertexArray[j]; // Now start and end correspond to the vertices
// Cases 1 and 4 - the endpoint is inside the boundary
// Cases 1 and 4 - the endpoint is inside the boundary
if (pointInsideBoundary(end,clipBoundary)) {
// Case 1 - Both inside
if (pointInsideBoundary(start, clipBoundary)) {
@ -409,14 +411,14 @@ void PolygonClip::sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::ve
appendPoint(end, outLength, outVertexArray);
}
} else { // Cases 2 and 3 - end is outside
if (pointInsideBoundary(start, clipBoundary)) {
if (pointInsideBoundary(start, clipBoundary)) {
// Cases 2 - start is inside, end is outside
segmentIntersectsBoundary(start, end, clipBoundary, intersection);
appendPoint(intersection, outLength, outVertexArray);
} else {
// Case 3 - both are outside, No action
}
}
}
start = end; // Advance to next pair of vertices
}
}
@ -468,23 +470,23 @@ void PolygonClip::appendPoint(glm::vec2 newVertex, int& outLength, glm::vec2* ou
}
// The copyCleanArray() function sets the resulting polygon of the previous step up to be the input polygon for next step of the
// clipping algorithm. As the Sutherland-Hodgman algorithm is a polygon clipping algorithm, it does not handle line
// clipping algorithm. As the Sutherland-Hodgman algorithm is a polygon clipping algorithm, it does not handle line
// clipping very well. The modification so that lines may be clipped as well as polygons is included in this function.
// when completed vertexArrayA will be ready for output and/or next step of clipping
// when completed vertexArrayA will be ready for output and/or next step of clipping
void PolygonClip::copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB) {
// Fix lines: they will come back with a length of 3, from an original of length of 2
if ((lengthA == 2) && (lengthB == 3)) {
// The first vertex should be copied as is.
vertexArrayA[0] = vertexArrayB[0];
// The first vertex should be copied as is.
vertexArrayA[0] = vertexArrayB[0];
// If the first two vertices of the "B" array are same, then collapse them down to be the 2nd vertex
if (vertexArrayB[0].x == vertexArrayB[1].x) {
vertexArrayA[1] = vertexArrayB[2];
} else {
} else {
// Otherwise the first vertex should be the same as third vertex
vertexArrayA[1] = vertexArrayB[1];
}
lengthA=2;
} else {
} else {
// for all other polygons, then just copy the vertexArrayB to vertextArrayA for next step
lengthA = lengthB;
for (int i = 0; i < lengthB; i++) {
@ -537,3 +539,21 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
return false;
}
void swingTwistDecomposition(const glm::quat& rotation,
const glm::vec3& direction,
glm::quat& swing,
glm::quat& twist) {
// direction MUST be normalized else the decomposition will be inaccurate
assert(fabsf(glm::length2(direction) - 1.0f) < 1.0e-4f);
// the twist part has an axis (imaginary component) that is parallel to direction argument
glm::vec3 axisOfRotation(rotation.x, rotation.y, rotation.z);
glm::vec3 twistImaginaryPart = glm::dot(direction, axisOfRotation) * direction;
// and a real component that is relatively proportional to rotation's real component
twist = glm::normalize(glm::quat(rotation.w, twistImaginaryPart.x, twistImaginaryPart.y, twistImaginaryPart.z));
// once twist is known we can solve for swing:
// rotation = swing * twist --> swing = rotation * invTwist
swing = rotation * glm::inverse(twist);
}

40
libraries/shared/src/GeometryUtil.h
View file

@ -22,7 +22,7 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
/// \param sphereRadius the radius of the sphere
/// \param penetration[out] the displacement that would move the point out of penetration with the sphere
/// \return true if point is inside sphere, otherwise false
bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection,
bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection,
float sphereRadius, glm::vec3& penetration);
bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius,
@ -33,7 +33,7 @@ bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereC
bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius,
const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration);
bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration);
@ -42,14 +42,14 @@ bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRad
bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart,
const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration);
bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius,
bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd,
float startRadius, float endRadius, glm::vec3& penetration);
bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec4& plane, glm::vec3& penetration);
/// Computes the penetration between a sphere and a disk.
/// \param sphereCenter center of sphere
/// \param sphereRadius radius of sphere
@ -58,8 +58,8 @@ bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadiu
/// \param diskNormal normal of disk plan
/// \param penetration[out] the depth that the sphere penetrates the disk
/// \return true if sphere touches disk (does not handle collisions with disk edge)
bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
bool findSphereDiskPenetration(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& diskCenter, float diskRadius, float diskThickness, const glm::vec3& diskNormal,
glm::vec3& penetration);
bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
@ -79,9 +79,19 @@ bool findRayCapsuleIntersection(const glm::vec3& origin, const glm::vec3& direct
bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::quat& rotation,
const glm::vec3& position, const glm::vec2& dimensions, float& distance);
bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance);
/// \brief decomposes rotation into its components such that: rotation = swing * twist
/// \param rotation[in] rotation to decompose
/// \param direction[in] normalized axis about which the twist happens (typically original direction before rotation applied)
/// \param swing[out] the swing part of rotation
/// \param twist[out] the twist part of rotation
void swingTwistDecomposition(const glm::quat& rotation,
const glm::vec3& direction, // must be normalized
glm::quat& swing,
glm::quat& twist);
class Triangle {
public:
glm::vec3 v0;
@ -89,11 +99,11 @@ public:
glm::vec3 v2;
};
inline bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
inline bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
const Triangle& triangle, float& distance) {
return findRayTriangleIntersection(origin, direction, triangle.v0, triangle.v1, triangle.v2, distance);
}
bool doLineSegmentsIntersect(glm::vec2 r1p1, glm::vec2 r1p2, glm::vec2 r2p1, glm::vec2 r2p2);
bool isOnSegment(float xi, float yi, float xj, float yj, float xk, float yk);
@ -117,15 +127,15 @@ public:
static const glm::vec2 TOP_RIGHT_CLIPPING_WINDOW;
static const glm::vec2 BOTTOM_LEFT_CLIPPING_WINDOW;
static const glm::vec2 BOTTOM_RIGHT_CLIPPING_WINDOW;
private:
static void sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
static void sutherlandHodgmanPolygonClip(glm::vec2* inVertexArray, glm::vec2* outVertexArray,
int inLength, int& outLength, const LineSegment2& clipBoundary);
static bool pointInsideBoundary(const glm::vec2& testVertex, const LineSegment2& clipBoundary);
static void segmentIntersectsBoundary(const glm::vec2& first, const glm::vec2& second,
static void segmentIntersectsBoundary(const glm::vec2& first, const glm::vec2& second,
const LineSegment2& clipBoundary, glm::vec2& intersection);
static void appendPoint(glm::vec2 newVertex, int& outLength, glm::vec2* outVertexArray);

12
tests/QTestExtensions.h
View file

@ -57,10 +57,10 @@ QString QTest_generateCompareFailureMessage (
QString s1 = actual_expr, s2 = expected_expr;
int pad1_ = qMax(s2.length() - s1.length(), 0);
int pad2_ = qMax(s1.length() - s2.length(), 0);
QString pad1 = QString(")").rightJustified(pad1_, ' ');
QString pad2 = QString(")").rightJustified(pad2_, ' ');
QString msg;
QTextStream stream (&msg);
stream << failMessage << "\n\t"
@ -88,10 +88,10 @@ QString QTest_generateCompareFailureMessage (
QString s1 = actual_expr, s2 = expected_expr;
int pad1_ = qMax(s2.length() - s1.length(), 0);
int pad2_ = qMax(s1.length() - s2.length(), 0);
QString pad1 = QString("): ").rightJustified(pad1_, ' ');
QString pad2 = QString("): ").rightJustified(pad2_, ' ');
QString msg;
QTextStream stream (&msg);
stream << failMessage << "\n\t"
@ -168,7 +168,7 @@ bool QTest_compareWithAbsError(
int line, const char* file,
const V& epsilon
) {
if (abs(getErrorDifference(actual, expected)) > abs(epsilon)) {
if (fabsf(getErrorDifference(actual, expected)) > fabsf(epsilon)) {
QTest_failWithMessage(
"Compared values are not the same (fuzzy compare)",
actual, expected, actual_expr, expected_expr, line, file,
@ -260,7 +260,7 @@ do { \
struct ByteData {
ByteData (const char* data, size_t length)
ByteData (const char* data, size_t length)
: data(data), length(length) {}
const char* data;
size_t length;

332
tests/animation/src/RotationConstraintTests.cpp Normal file
View file

@ -0,0 +1,332 @@
//
// RotationConstraintTests.cpp
// tests/rig/src
//
// Copyright 2015 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 "RotationConstraintTests.h"
#include <glm/glm.hpp>
#include <ElbowConstraint.h>
#include <NumericalConstants.h>
#include <SwingTwistConstraint.h>
// HACK -- these helper functions need to be defined BEFORE including magic inside QTestExtensions.h
// TODO: fix QTestExtensions so we don't need to do this in every test.
// Computes the error value between two quaternions (using glm::dot)
float getErrorDifference(const glm::quat& a, const glm::quat& b) {
return fabsf(glm::dot(a, b)) - 1.0f;
}
QTextStream& operator<<(QTextStream& stream, const glm::quat& q) {
return stream << "glm::quat { " << q.x << ", " << q.y << ", " << q.z << ", " << q.w << " }";
}
// Produces a relative error test for float usable QCOMPARE_WITH_LAMBDA.
inline auto errorTest (float actual, float expected, float acceptableRelativeError)
-> std::function<bool ()> {
return [actual, expected, acceptableRelativeError] () {
if (fabsf(expected) <= acceptableRelativeError) {
return fabsf(actual - expected) < fabsf(acceptableRelativeError);
}
return fabsf((actual - expected) / expected) < fabsf(acceptableRelativeError);
};
}
#include "../QTestExtensions.h"
#define QCOMPARE_WITH_RELATIVE_ERROR(actual, expected, relativeError) \
QCOMPARE_WITH_LAMBDA(actual, expected, errorTest(actual, expected, relativeError))
QTEST_MAIN(RotationConstraintTests)
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;
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;
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;
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;
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;
}
}
}

24
tests/animation/src/RotationConstraintTests.h Normal file
View file

@ -0,0 +1,24 @@
//
// RotationConstraintTests.h
// tests/rig/src
//
// Copyright 2015 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_RotationConstraintTests_h
#define hifi_RotationConstraintTests_h
#include <QtTest/QtTest>
class RotationConstraintTests : public QObject {
Q_OBJECT
private slots:
void testElbowConstraint();
void testSwingTwistConstraint();
};
#endif // hifi_RotationConstraintTests_h

1936
tests/physics/src/ShapeColliderTests.cpp
View file

File diff suppressed because it is too large Load diff

53
tests/physics/src/ShapeColliderTests.h
View file

@ -1,53 +0,0 @@
//
// ShapeColliderTests.h
// tests/physics/src
//
// Created by Andrew Meadows on 02/21/2014.
// 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_ShapeColliderTests_h
#define hifi_ShapeColliderTests_h
#include <QtTest/QtTest>
class ShapeColliderTests : public QObject {
Q_OBJECT
private slots:
void initTestCase();
void sphereMissesSphere();
void sphereTouchesSphere();
void sphereMissesCapsule();
void sphereTouchesCapsule();
void capsuleMissesCapsule();
void capsuleTouchesCapsule();
void sphereTouchesAACubeFaces();
void sphereTouchesAACubeEdges();
void sphereTouchesAACubeCorners();
void sphereMissesAACube();
void capsuleMissesAACube();
void capsuleTouchesAACube();
void rayHitsSphere();
void rayBarelyHitsSphere();
void rayBarelyMissesSphere();
void rayHitsCapsule();
void rayMissesCapsule();
void rayHitsPlane();
void rayMissesPlane();
void rayHitsAACube();
void rayMissesAACube();
void measureTimeOfCollisionDispatch();
};
#endif // hifi_ShapeColliderTests_h

81
tests/shared/src/GeometryUtilTests.cpp
View file

@ -41,11 +41,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
glm::vec3 rectCenter(0.0f, 0.0f, 0.0f);
glm::quat rectRotation = glm::quat(); // identity
// create points for generating rays that hit the plane and don't
glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
float delta = 0.1f;
{ // verify hit
glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
float delta = 0.1f;
glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.x - delta) * xAxis);
glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
float expectedDistance = glm::length(rayEnd - rayStart);
@ -57,6 +55,8 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
}
{ // verify miss
glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
float delta = 0.1f;
glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.y + delta) * yAxis);
glm::vec3 rayMissDirection = glm::normalize(rayEnd - rayStart);
float distance = FLT_MAX;
@ -67,9 +67,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
{ // hit with co-planer line
float yFraction = 0.25f;
rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - yFraction * rectDimensions.y * yAxis);
glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
float expectedDistance = rectDimensions.x;
@ -81,9 +81,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
{ // miss with co-planer line
float yFraction = 0.75f;
rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter + rectRotation * (- rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
float distance = FLT_MAX;
@ -134,7 +134,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
float yFraction = 0.25f;
rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
float expectedDistance = rectDimensions.x;
@ -148,7 +148,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
float yFraction = 0.75f;
rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter + rectRotation * (-rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
float distance = FLT_MAX;
@ -158,3 +158,64 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
}
}
void GeometryUtilTests::testTwistSwingDecomposition() {
// for each twist and swing angle pair:
// (a) compute twist and swing input components
// (b) compose the total rotation
// (c) decompose the total rotation
// (d) compare decomposed values with input components
glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
glm::vec3 twistAxis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); // can be anything but xAxis
glm::vec3 initialSwingAxis = glm::normalize(glm::cross(xAxis, twistAxis)); // initialSwingAxis must be perp to twistAxis
const int numTwists = 6;
const int numSwings = 7;
const int numSwingAxes = 5;
const float smallAngle = PI / 100.0f;
const float maxTwist = PI;
const float minTwist = -PI;
const float minSwing = 0.0f;
const float maxSwing = PI;
const float deltaTwist = (maxTwist - minTwist - 2.0f * smallAngle) / (float)(numTwists - 1);
const float deltaSwing = (maxSwing - minSwing - 2.0f * smallAngle) / (float)(numSwings - 1);
for (float twist = minTwist + smallAngle; twist < maxTwist; twist += deltaTwist) {
// compute twist component
glm::quat twistRotation = glm::angleAxis(twist, twistAxis);
float deltaTheta = TWO_PI / (numSwingAxes - 1);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
// compute the swingAxis
glm::quat thetaRotation = glm::angleAxis(theta, twistAxis);
glm::vec3 swingAxis = thetaRotation * initialSwingAxis;
for (float swing = minSwing + smallAngle; swing < maxSwing; swing += deltaSwing) {
// compute swing component
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
// compose
glm::quat totalRotation = swingRotation * twistRotation;
// decompose
glm::quat measuredTwistRotation;
glm::quat measuredSwingRotation;
swingTwistDecomposition(totalRotation, twistAxis, measuredSwingRotation, measuredTwistRotation);
// dot decomposed with components
float twistDot = fabsf(glm::dot(twistRotation, measuredTwistRotation));
float swingDot = fabsf(glm::dot(swingRotation, measuredSwingRotation));
// the dot products should be very close to 1.0
const float MIN_ERROR = 1.0e-6f;
QCOMPARE_WITH_ABS_ERROR(1.0f, twistDot, MIN_ERROR);
QCOMPARE_WITH_ABS_ERROR(1.0f, swingDot, MIN_ERROR);
}
}
}
}

1
tests/shared/src/GeometryUtilTests.h
View file

@ -20,6 +20,7 @@ class GeometryUtilTests : public QObject {
private slots:
void testLocalRayRectangleIntersection();
void testWorldRayRectangleIntersection();
void testTwistSwingDecomposition();
};
float getErrorDifference(const float& a, const float& b);