Mirrors/overte-AleziaKurdis
3
0
Fork 0
mirror of https://github.com/AleziaKurdis/overte.git synced 2025-04-08 06:32:35 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #2190 from AndrewMeadows/shapes

Browse source 
Collision Shapes
This commit is contained in:
Philip Rosedale 2014-03-05 17:59:42 -08:00
commit c2a63c4206
30 changed files with 2234 additions and 123 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
interface/src/avatar/Avatar.cpp
View file

@ -484,10 +484,19 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc
bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius,
CollisionList& collisions, int skeletonSkipIndex) {
// Temporarily disabling collisions against the skeleton because the collision proxies up
// near the neck are bad and prevent the hand from hitting the face.
//return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, 1.0f, skeletonSkipIndex);
return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions);
return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, skeletonSkipIndex);
// Temporarily disabling collisions against the head because most of its collision proxies are bad.
//return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions);
}
bool Avatar::findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions) {
_skeletonModel.updateShapePositions();
bool collided = _skeletonModel.findCollisions(shapes, collisions);
Model& headModel = getHead()->getFaceModel();
headModel.updateShapePositions();
collided = headModel.findCollisions(shapes, collisions);
return collided;
}
bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) {

5
interface/src/avatar/Avatar.h
View file

@ -99,6 +99,11 @@ public:
bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
/// \param shapes list of shapes to collide against avatar
/// \param collisions list to store collision results
/// \return true if at least one shape collided with avatar
bool findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions);
/// Checks for penetration between the described sphere and the avatar.
/// \param penetratorCenter the center of the penetration test sphere
/// \param penetratorRadius the radius of the penetration test sphere

86
interface/src/avatar/Hand.cpp
View file

@ -96,7 +96,7 @@ void Hand::playSlaps(PalmData& palm, Avatar* avatar)
const float MAX_COLLISIONS_PER_AVATAR = 32;
static CollisionList handCollisions(MAX_COLLISIONS_PER_AVATAR);
void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
void Hand::collideAgainstAvatarOld(Avatar* avatar, bool isMyHand) {
if (!avatar || avatar == _owningAvatar) {
// don't collide with our own hands (that is done elsewhere)
return;
@ -117,17 +117,9 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
for (int j = 0; j < handCollisions.size(); ++j) {
CollisionInfo* collision = handCollisions.getCollision(j);
if (isMyHand) {
if (!avatar->collisionWouldMoveAvatar(*collision)) {
// we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is
// not expected to respond to the collision (hand hit unmovable part of their Avatar)
totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
}
} else {
// when !isMyHand then avatar is MyAvatar and we apply the collision
// which might not do anything (hand hit unmovable part of MyAvatar) however
// we don't resolve the hand's penetration because we expect the remote
// simulation to do the right thing.
avatar->applyCollision(*collision);
// we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is
// not expected to respond to the collision (hand hit unmovable part of their Avatar)
totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
}
}
}
@ -138,6 +130,66 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
}
}
void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
if (!avatar || avatar == _owningAvatar) {
// don't collide hands against ourself (that is done elsewhere)
return;
}
// 2 = NUM_HANDS
int palmIndices[2];
getLeftRightPalmIndices(*palmIndices, *(palmIndices + 1));
const SkeletonModel& skeletonModel = _owningAvatar->getSkeletonModel();
int jointIndices[2];
jointIndices[0] = skeletonModel.getLeftHandJointIndex();
jointIndices[1] = skeletonModel.getRightHandJointIndex();
palmIndices[1] = -1; // adebug temporarily disable right hand
jointIndices[1] = -1; // adebug temporarily disable right hand
for (size_t i = 0; i < 1; i++) {
int palmIndex = palmIndices[i];
int jointIndex = jointIndices[i];
if (palmIndex == -1 || jointIndex == -1) {
continue;
}
PalmData& palm = _palms[palmIndex];
if (!palm.isActive()) {
continue;
}
if (isMyHand && Menu::getInstance()->isOptionChecked(MenuOption::PlaySlaps)) {
playSlaps(palm, avatar);
}
handCollisions.clear();
QVector<const Shape*> shapes;
skeletonModel.getHandShapes(jointIndex, shapes);
bool collided = isMyHand ? avatar->findCollisions(shapes, handCollisions) : avatar->findCollisions(shapes, handCollisions);
if (collided) {
//if (avatar->findCollisions(shapes, handCollisions)) {
glm::vec3 averagePenetration;
glm::vec3 averageContactPoint;
for (int j = 0; j < handCollisions.size(); ++j) {
CollisionInfo* collision = handCollisions.getCollision(j);
averagePenetration += collision->_penetration;
averageContactPoint += collision->_contactPoint;
}
averagePenetration /= float(handCollisions.size());
if (isMyHand) {
// our hand against other avatar
// for now we resolve it to test shapes/collisions
// TODO: only partially resolve this penetration
palm.addToPosition(-averagePenetration);
} else {
// someone else's hand against MyAvatar
// TODO: submit collision info to MyAvatar which should lean accordingly
averageContactPoint /= float(handCollisions.size());
}
}
}
}
void Hand::collideAgainstOurself() {
if (!Menu::getInstance()->isOptionChecked(MenuOption::HandsCollideWithSelf)) {
return;
@ -147,27 +199,27 @@ void Hand::collideAgainstOurself() {
getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
float scaledPalmRadius = PALM_COLLISION_RADIUS * _owningAvatar->getScale();
const Model& skeletonModel = _owningAvatar->getSkeletonModel();
for (size_t i = 0; i < getNumPalms(); i++) {
PalmData& palm = getPalms()[i];
if (!palm.isActive()) {
continue;
}
const Model& skeletonModel = _owningAvatar->getSkeletonModel();
}
// ignoring everything below the parent of the parent of the last free joint
int skipIndex = skeletonModel.getParentJointIndex(skeletonModel.getParentJointIndex(
skeletonModel.getLastFreeJointIndex((i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
(i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1)));
handCollisions.clear();
glm::vec3 totalPenetration;
if (_owningAvatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, handCollisions, skipIndex)) {
glm::vec3 totalPenetration;
for (int j = 0; j < handCollisions.size(); ++j) {
CollisionInfo* collision = handCollisions.getCollision(j);
totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
}
// resolve penetration
palm.addToPosition(-totalPenetration);
}
// resolve penetration
palm.addToPosition(-totalPenetration);
}
}

1
interface/src/avatar/Hand.h
View file

@ -56,6 +56,7 @@ public:
const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;}
const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;}
void collideAgainstAvatarOld(Avatar* avatar, bool isMyHand);
void collideAgainstAvatar(Avatar* avatar, bool isMyHand);
void collideAgainstOurself();

20
interface/src/avatar/MyAvatar.cpp
View file

@ -458,16 +458,16 @@ void MyAvatar::render(bool forceRenderHead, bool avatarOnly) {
return; // exit early
}
// render body
if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionProxies)) {
_skeletonModel.renderCollisionProxies(1.f);
}
if (Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionProxies)) {
_skeletonModel.renderCollisionProxies(1.f);
}
if (Menu::getInstance()->isOptionChecked(MenuOption::Avatars)) {
renderBody(forceRenderHead);
}
// render body
if (Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionProxies)) {
_skeletonModel.renderCollisionProxies(0.8f);
}
if (Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionProxies)) {
getHead()->getFaceModel().renderCollisionProxies(0.8f);
}
setShowDisplayName(!avatarOnly);
if (avatarOnly) {
return;
@ -941,6 +941,11 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
}
float theirBoundingRadius = avatar->getBoundingRadius();
if (distance < myBoundingRadius + theirBoundingRadius) {
_skeletonModel.updateShapePositions();
Model& headModel = getHead()->getFaceModel();
headModel.updateShapePositions();
/* TODO: Andrew to fix Avatar-Avatar body collisions
Extents theirStaticExtents = _skeletonModel.getStaticExtents();
glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum;
float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
@ -952,6 +957,7 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
// move the avatar out by half the penetration
setPosition(_position - 0.5f * penetration);
}
*/
// collide our hands against them
getHand()->collideAgainstAvatar(avatar, true);

11
interface/src/avatar/SkeletonModel.cpp
View file

@ -73,6 +73,17 @@ bool SkeletonModel::render(float alpha) {
return true;
}
void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const {
if (jointIndex == -1) {
return;
}
if (jointIndex == getLeftHandJointIndex()
|| jointIndex == getRightHandJointIndex()) {
// TODO: also add fingers and other hand-parts
shapes.push_back(_shapes[jointIndex]);
}
}
class IndexValue {
public:
int index;

4
interface/src/avatar/SkeletonModel.h
View file

@ -24,6 +24,10 @@ public:
void simulate(float deltaTime, bool delayLoad = false);
bool render(float alpha);
/// \param jointIndex index of hand joint
/// \param shapes[out] list in which is stored pointers to hand shapes
void getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const;
protected:

152
interface/src/renderer/FBXReader.cpp
View file

@ -6,6 +6,7 @@
// Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
//
#include <iostream>
#include <QBuffer>
#include <QDataStream>
#include <QIODevice>
@ -21,6 +22,7 @@
#include <OctalCode.h>
#include <GeometryUtil.h>
#include <Shape.h>
#include <VoxelTree.h>
#include "FBXReader.h"
@ -28,6 +30,22 @@
using namespace std;
void Extents::reset() {
minimum = glm::vec3(FLT_MAX);
maximum = glm::vec3(-FLT_MAX);
}
bool Extents::containsPoint(const glm::vec3& point) const {
return (point.x >= minimum.x && point.x <= maximum.x
&& point.y >= minimum.y && point.y <= maximum.y
&& point.z >= minimum.z && point.z <= maximum.z);
}
void Extents::addPoint(const glm::vec3& point) {
minimum = glm::min(minimum, point);
maximum = glm::max(maximum, point);
}
static int fbxGeometryMetaTypeId = qRegisterMetaType<FBXGeometry>();
template<class T> QVariant readBinaryArray(QDataStream& in) {
@ -538,8 +556,9 @@ public:
FBXBlendshape blendshape;
};
void printNode(const FBXNode& node, int indent) {
QByteArray spaces(indent, ' ');
void printNode(const FBXNode& node, int indentLevel) {
int indentLength = 2;
QByteArray spaces(indentLevel * indentLength, ' ');
QDebug nodeDebug = qDebug();
nodeDebug.nospace() << spaces.data() << node.name.data() << ": ";
@ -548,7 +567,7 @@ void printNode(const FBXNode& node, int indent) {
}
foreach (const FBXNode& child, node.children) {
printNode(child, indent + 1);
printNode(child, indentLevel + 1);
}
}
@ -842,6 +861,21 @@ QString getString(const QVariant& value) {
return list.isEmpty() ? value.toString() : list.at(0).toString();
}
class JointShapeInfo {
public:
JointShapeInfo() : numVertices(0), numProjectedVertices(0), averageVertex(0.f), boneBegin(0.f), averageRadius(0.f) {
extents.reset();
}
// NOTE: the points here are in the "joint frame" which has the "jointEnd" at the origin
int numVertices; // num vertices from contributing meshes
int numProjectedVertices; // num vertices that successfully project onto bone axis
Extents extents; // max and min extents of mesh vertices (in joint frame)
glm::vec3 averageVertex; // average of all mesh vertices (in joint frame)
glm::vec3 boneBegin; // parent joint location (in joint frame)
float averageRadius; // average distance from mesh points to averageVertex
};
FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) {
QHash<QString, ExtractedMesh> meshes;
QVector<ExtractedBlendshape> blendshapes;
@ -1250,9 +1284,15 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
}
joint.boneRadius = 0.0f;
joint.inverseBindRotation = joint.inverseDefaultRotation;
joint.name = model.name;
joint.shapePosition = glm::vec3(0.f);
joint.shapeType = Shape::UNKNOWN_SHAPE;
geometry.joints.append(joint);
geometry.jointIndices.insert(model.name, geometry.joints.size());
}
// for each joint we allocate a JointShapeInfo in which we'll store collision shape info
QVector<JointShapeInfo> jointShapeInfos;
jointShapeInfos.resize(geometry.joints.size());
// find our special joints
geometry.leftEyeJointIndex = modelIDs.indexOf(jointEyeLeftID);
@ -1274,10 +1314,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
geometry.neckPivot = glm::vec3(transform[3][0], transform[3][1], transform[3][2]);
}
geometry.bindExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
geometry.bindExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
geometry.staticExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
geometry.staticExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
geometry.bindExtents.reset();
geometry.staticExtents.reset();
QVariantHash springs = mapping.value("spring").toHash();
QVariant defaultSpring = springs.value("default");
@ -1393,8 +1431,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
// update the bind pose extents
glm::vec3 bindTranslation = extractTranslation(geometry.offset * joint.bindTransform);
geometry.bindExtents.minimum = glm::min(geometry.bindExtents.minimum, bindTranslation);
geometry.bindExtents.maximum = glm::max(geometry.bindExtents.maximum, bindTranslation);
geometry.bindExtents.addPoint(bindTranslation);
}
}
@ -1407,11 +1444,6 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
cluster.jointIndex = 0;
}
extracted.mesh.clusters.append(cluster);
// BUG: joints that fall into this context do not get their bindTransform and
// inverseBindRotation data members properly set. This causes bad boneRadius
// and boneLength calculations for collision proxies. Affected joints are usually:
// hair, teeth, tongue. I tried to figure out how to fix this but was going
// crosseyed trying to understand FBX so I gave up for the time being -- Andrew.
}
// whether we're skinned depends on how many clusters are attached
@ -1428,20 +1460,26 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
const FBXCluster& fbxCluster = extracted.mesh.clusters.at(i);
int jointIndex = fbxCluster.jointIndex;
FBXJoint& joint = geometry.joints[jointIndex];
glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform;
glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh));
glm::vec3 boneEnd = extractTranslation(transformJointToMesh);
glm::vec3 boneBegin = boneEnd;
glm::vec3 boneDirection;
float boneLength;
if (joint.parentIndex != -1) {
boneDirection = boneEnd - extractTranslation(inverseModelTransform *
geometry.joints[joint.parentIndex].bindTransform);
boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
boneDirection = boneEnd - boneBegin;
boneLength = glm::length(boneDirection);
if (boneLength > EPSILON) {
boneDirection /= boneLength;
}
}
float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd));
bool jointIsStatic = joint.freeLineage.isEmpty();
glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform);
float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
float totalWeight = 0.0f;
for (int j = 0; j < cluster.indices.size(); j++) {
int oldIndex = cluster.indices.at(j);
@ -1455,13 +1493,17 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
float proj = glm::dot(boneDirection, vertex - boneEnd);
if (proj < 0.0f && proj > -boneLength) {
joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
vertex, boneEnd + boneDirection * proj));
joint.boneRadius = glm::max(joint.boneRadius,
radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj));
++jointShapeInfo.numProjectedVertices;
}
glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
jointShapeInfo.extents.addPoint(vertexInJointFrame);
jointShapeInfo.averageVertex += vertexInJointFrame;
++jointShapeInfo.numVertices;
if (jointIsStatic) {
// expand the extents of static (nonmovable) joints
geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation);
geometry.staticExtents.maximum = glm::max(geometry.staticExtents.maximum, vertex + jointTranslation);
geometry.staticExtents.addPoint(vertex + jointTranslation);
}
}
@ -1484,24 +1526,47 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
} else {
int jointIndex = maxJointIndex;
FBXJoint& joint = geometry.joints[jointIndex];
glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
JointShapeInfo& jointShapeInfo = jointShapeInfos[jointIndex];
glm::mat4 transformJointToMesh = inverseModelTransform * joint.bindTransform;
glm::quat rotateMeshToJoint = glm::inverse(extractRotation(transformJointToMesh));
glm::vec3 boneEnd = extractTranslation(transformJointToMesh);
glm::vec3 boneBegin = boneEnd;
glm::vec3 boneDirection;
float boneLength;
if (joint.parentIndex != -1) {
boneDirection = boneEnd - extractTranslation(inverseModelTransform *
geometry.joints[joint.parentIndex].bindTransform);
boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
boneDirection = boneEnd - boneBegin;
boneLength = glm::length(boneDirection);
if (boneLength > EPSILON) {
boneDirection /= boneLength;
}
}
float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
jointShapeInfo.boneBegin = rotateMeshToJoint * (radiusScale * (boneBegin - boneEnd));
glm::vec3 averageVertex(0.f);
foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
float proj = glm::dot(boneDirection, vertex - boneEnd);
if (proj < 0.0f && proj > -boneLength) {
joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
vertex, boneEnd + boneDirection * proj));
joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(vertex, boneEnd + boneDirection * proj));
++jointShapeInfo.numProjectedVertices;
}
glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
jointShapeInfo.extents.addPoint(vertexInJointFrame);
jointShapeInfo.averageVertex += vertexInJointFrame;
averageVertex += vertex;
}
int numVertices = extracted.mesh.vertices.size();
jointShapeInfo.numVertices = numVertices;
if (numVertices > 0) {
averageVertex /= float(jointShapeInfo.numVertices);
float averageRadius = 0.f;
foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
averageRadius += glm::distance(vertex, averageVertex);
}
jointShapeInfo.averageRadius = averageRadius * radiusScale;
}
}
extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);
@ -1551,6 +1616,39 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
geometry.meshes.append(extracted.mesh);
}
// now that all joints have been scanned, compute a collision shape for each joint
glm::vec3 defaultCapsuleAxis(0.f, 1.f, 0.f);
for (int i = 0; i < geometry.joints.size(); ++i) {
FBXJoint& joint = geometry.joints[i];
JointShapeInfo& jointShapeInfo = jointShapeInfos[i];
// we use a capsule if the joint ANY mesh vertices successfully projected onto the bone
// AND its boneRadius is not too close to zero
bool collideLikeCapsule = jointShapeInfo.numProjectedVertices > 0
&& glm::length(jointShapeInfo.boneBegin) > EPSILON;
if (collideLikeCapsule) {
joint.shapeRotation = rotationBetween(defaultCapsuleAxis, jointShapeInfo.boneBegin);
joint.shapePosition = 0.5f * jointShapeInfo.boneBegin;
joint.shapeType = Shape::CAPSULE_SHAPE;
} else {
// collide the joint like a sphere
if (jointShapeInfo.numVertices > 0) {
jointShapeInfo.averageVertex /= float(jointShapeInfo.numVertices);
joint.shapePosition = jointShapeInfo.averageVertex;
} else {
joint.shapePosition = glm::vec3(0.f);
joint.shapeType = Shape::SPHERE_SHAPE;
}
if (jointShapeInfo.numProjectedVertices == 0
&& jointShapeInfo.numVertices > 0) {
// the bone projection algorithm was not able to compute the joint radius
// so we use an alternative measure
jointShapeInfo.averageRadius /= float(jointShapeInfo.numVertices);
joint.boneRadius = jointShapeInfo.averageRadius;
}
}
}
geometry.palmDirection = parseVec3(mapping.value("palmDirection", "0, -1, 0").toString());
// process attachments

29
interface/src/renderer/FBXReader.h
View file

@ -25,6 +25,23 @@ typedef QList<FBXNode> FBXNodeList;
/// The names of the blendshapes expected by Faceshift, terminated with an empty string.
extern const char* FACESHIFT_BLENDSHAPES[];
class Extents {
public:
/// set minimum and maximum to FLT_MAX and -FLT_MAX respectively
void reset();
/// \param point new point to compare against existing limits
/// compare point to current limits and expand them if necessary to contain point
void addPoint(const glm::vec3& point);
/// \param point
/// \return true if point is within current limits
bool containsPoint(const glm::vec3& point) const;
glm::vec3 minimum;
glm::vec3 maximum;
};
/// A node within an FBX document.
class FBXNode {
public:
@ -64,8 +81,13 @@ public:
glm::quat inverseDefaultRotation;
glm::quat inverseBindRotation;
glm::mat4 bindTransform;
QString name; // temp field for debugging
glm::vec3 shapePosition; // in joint frame
glm::quat shapeRotation; // in joint frame
int shapeType;
};
/// A single binding to a joint in an FBX document.
class FBXCluster {
public:
@ -125,13 +147,6 @@ public:
glm::vec3 scale;
};
class Extents {
public:
glm::vec3 minimum;
glm::vec3 maximum;
};
/// A set of meshes extracted from an FBX document.
class FBXGeometry {
public:

171
interface/src/renderer/Model.cpp
View file

@ -13,10 +13,15 @@
#include "Application.h"
#include "Model.h"
#include <SphereShape.h>
#include <CapsuleShape.h>
#include <ShapeCollider.h>
using namespace std;
Model::Model(QObject* parent) :
QObject(parent),
_shapesAreDirty(true),
_lodDistance(0.0f),
_pupilDilation(0.0f) {
// we may have been created in the network thread, but we live in the main thread
@ -100,6 +105,51 @@ void Model::reset() {
}
}
void Model::clearShapes() {
for (int i = 0; i < _shapes.size(); ++i) {
delete _shapes[i];
}
_shapes.clear();
}
void Model::createCollisionShapes() {
clearShapes();
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float uniformScale = extractUniformScale(_scale);
for (int i = 0; i < _jointStates.size(); i++) {
const FBXJoint& joint = geometry.joints[i];
glm::vec3 meshCenter = _jointStates[i].combinedRotation * joint.shapePosition;
glm::vec3 position = _rotation * (extractTranslation(_jointStates[i].transform) + uniformScale * meshCenter) + _translation;
float radius = uniformScale * joint.boneRadius;
if (joint.shapeType == Shape::CAPSULE_SHAPE) {
float halfHeight = 0.5f * uniformScale * joint.distanceToParent;
CapsuleShape* shape = new CapsuleShape(radius, halfHeight);
shape->setPosition(position);
_shapes.push_back(shape);
} else {
SphereShape* shape = new SphereShape(radius, position);
_shapes.push_back(shape);
}
}
}
void Model::updateShapePositions() {
if (_shapesAreDirty && _shapes.size() == _jointStates.size()) {
float uniformScale = extractUniformScale(_scale);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
for (int i = 0; i < _jointStates.size(); i++) {
const FBXJoint& joint = geometry.joints[i];
// shape position and rotation need to be in world-frame
glm::vec3 jointToShapeOffset = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition);
glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation;
_shapes[i]->setPosition(worldPosition);
_shapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
}
_shapesAreDirty = false;
}
}
void Model::simulate(float deltaTime, bool delayLoad) {
// update our LOD
QVector<JointState> newJointStates = updateGeometry(delayLoad);
@ -128,6 +178,7 @@ void Model::simulate(float deltaTime, bool delayLoad) {
_attachments.append(model);
}
_resetStates = true;
createCollisionShapes();
}
// update the world space transforms for all joints
@ -454,20 +505,28 @@ bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direct
return false;
}
bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius,
CollisionList& collisions, float boneScale, int skipIndex) const {
bool Model::findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions) {
bool collided = false;
const glm::vec3 relativeCenter = penetratorCenter - _translation;
for (int i = 0; i < shapes.size(); ++i) {
const Shape* theirShape = shapes[i];
for (int j = 0; j < _shapes.size(); ++j) {
const Shape* ourShape = _shapes[j];
if (ShapeCollider::shapeShape(theirShape, ourShape, collisions)) {
collided = true;
}
}
}
return collided;
}
bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius,
CollisionList& collisions, int skipIndex) {
bool collided = false;
updateShapePositions();
SphereShape sphere(sphereRadius, sphereCenter);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
glm::vec3 totalPenetration;
float radiusScale = extractUniformScale(_scale) * boneScale;
for (int i = 0; i < _jointStates.size(); i++) {
for (int i = 0; i < _shapes.size(); i++) {
const FBXJoint& joint = geometry.joints[i];
glm::vec3 end = extractTranslation(_jointStates[i].transform);
float endRadius = joint.boneRadius * radiusScale;
glm::vec3 start = end;
float startRadius = joint.boneRadius * radiusScale;
glm::vec3 bonePenetration;
if (joint.parentIndex != -1) {
if (skipIndex != -1) {
int ancestorIndex = joint.parentIndex;
@ -479,24 +538,13 @@ bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetr
} while (ancestorIndex != -1);
}
start = extractTranslation(_jointStates[joint.parentIndex].transform);
startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale;
}
if (findSphereCapsuleConePenetration(relativeCenter, penetratorRadius, start, end,
startRadius, endRadius, bonePenetration)) {
totalPenetration = addPenetrations(totalPenetration, bonePenetration);
CollisionInfo* collision = collisions.getNewCollision();
if (collision) {
collision->_type = MODEL_COLLISION;
collision->_data = (void*)(this);
collision->_flags = i;
collision->_contactPoint = penetratorCenter + penetratorRadius * glm::normalize(totalPenetration);
collision->_penetration = totalPenetration;
collided = true;
} else {
// collisions are full, so we might as well break
break;
}
if (ShapeCollider::shapeShape(&sphere, _shapes[i], collisions)) {
CollisionInfo* collision = collisions.getLastCollision();
collision->_type = MODEL_COLLISION;
collision->_data = (void*)(this);
collision->_flags = i;
collided = true;
}
outerContinue: ;
}
@ -504,6 +552,7 @@ bool Model::findSphereCollisions(const glm::vec3& penetratorCenter, float penetr
}
void Model::updateJointState(int index) {
_shapesAreDirty = true;
JointState& state = _jointStates[index];
const FBXGeometry& geometry = _geometry->getFBXGeometry();
const FBXJoint& joint = geometry.joints.at(index);
@ -702,34 +751,51 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
void Model::renderCollisionProxies(float alpha) {
glPushMatrix();
Application::getInstance()->loadTranslatedViewMatrix(_translation);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float uniformScale = extractUniformScale(_scale);
for (int i = 0; i < _jointStates.size(); i++) {
updateShapePositions();
const int BALL_SUBDIVISIONS = 10;
for (int i = 0; i < _shapes.size(); i++) {
glPushMatrix();
Shape* shape = _shapes[i];
glm::vec3 position = extractTranslation(_jointStates[i].transform);
glTranslatef(position.x, position.y, position.z);
glm::quat rotation;
getJointRotation(i, rotation);
glm::vec3 axis = glm::axis(rotation);
glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z);
glColor4f(0.75f, 0.75f, 0.75f, alpha);
float scaledRadius = geometry.joints[i].boneRadius * uniformScale;
const int BALL_SUBDIVISIONS = 10;
glutSolidSphere(scaledRadius, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
glPopMatrix();
int parentIndex = geometry.joints[i].parentIndex;
if (parentIndex != -1) {
Avatar::renderJointConnectingCone(extractTranslation(_jointStates[parentIndex].transform), position,
geometry.joints[parentIndex].boneRadius * uniformScale, scaledRadius);
if (shape->getType() == Shape::SPHERE_SHAPE) {
// shapes are stored in world-frame, so we have to transform into model frame
glm::vec3 position = shape->getPosition() - _translation;
glTranslatef(position.x, position.y, position.z);
const glm::quat& rotation = shape->getRotation();
glm::vec3 axis = glm::axis(rotation);
glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z);
// draw a grey sphere at shape position
glColor4f(0.75f, 0.75f, 0.75f, alpha);
glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
} else if (shape->getType() == Shape::CAPSULE_SHAPE) {
CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
// draw a blue sphere at the capsule endpoint
glm::vec3 endPoint;
capsule->getEndPoint(endPoint);
endPoint = endPoint - _translation;
glTranslatef(endPoint.x, endPoint.y, endPoint.z);
glColor4f(0.6f, 0.6f, 0.8f, alpha);
glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
// draw a yellow sphere at the capsule startpoint
glm::vec3 startPoint;
capsule->getStartPoint(startPoint);
startPoint = startPoint - _translation;
glm::vec3 axis = endPoint - startPoint;
glTranslatef(-axis.x, -axis.y, -axis.z);
glColor4f(0.8f, 0.8f, 0.6f, alpha);
glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
// draw a green cylinder between the two points
glm::vec3 origin(0.f);
glColor4f(0.6f, 0.8f, 0.6f, alpha);
Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius());
}
glPopMatrix();
}
glPopMatrix();
}
@ -847,6 +913,7 @@ void Model::deleteGeometry() {
_blendedVertexBufferIDs.clear();
_jointStates.clear();
_meshStates.clear();
clearShapes();
if (_geometry) {
_geometry->clearLoadPriority(this);

22
interface/src/renderer/Model.h
View file

@ -17,6 +17,8 @@
#include "ProgramObject.h"
#include "TextureCache.h"
class Shape;
/// A generic 3D model displaying geometry loaded from a URL.
class Model : public QObject {
Q_OBJECT
@ -52,6 +54,9 @@ public:
void init();
void reset();
void clearShapes();
void createCollisionShapes();
void updateShapePositions();
void simulate(float deltaTime, bool delayLoad = false);
bool render(float alpha);
@ -164,9 +169,14 @@ public:
glm::vec4 computeAverageColor() const;
bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
/// \param shapes list of pointers shapes to test against Model
/// \param collisions list to store collision results
/// \return true if at least one shape collided agains Model
bool findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions);
bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius,
CollisionList& collisions, float boneScale = 1.0f, int skipIndex = -1) const;
CollisionList& collisions, int skipIndex = -1);
void renderCollisionProxies(float alpha);
@ -189,13 +199,15 @@ protected:
class JointState {
public:
glm::vec3 translation;
glm::quat rotation;
glm::mat4 transform;
glm::quat combinedRotation;
glm::vec3 translation; // translation relative to parent
glm::quat rotation; // rotation relative to parent
glm::mat4 transform; // rotation to world frame + translation in model frame
glm::quat combinedRotation; // rotation from joint local to world frame
};
bool _shapesAreDirty;
QVector<JointState> _jointStates;
QVector<Shape*> _shapes;
class MeshState {
public:

2
libraries/avatars/src/HandData.h
View file

@ -89,7 +89,7 @@ public:
friend class AvatarData;
protected:
AvatarData* _owningAvatarData;
std::vector<PalmData> _palms;
std::vector<PalmData> _palms;
glm::quat getBaseOrientation() const;
glm::vec3 getBasePosition() const;

78
libraries/shared/src/CapsuleShape.cpp Normal file
View file

@ -0,0 +1,78 @@
//
// CapsuleShape.cpp
// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#include <iostream>
#include <glm/gtx/vector_angle.hpp>
#include "CapsuleShape.h"
#include "SharedUtil.h"
// default axis of CapsuleShape is Y-axis
const glm::vec3 localAxis(0.f, 1.f, 0.f);
CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE) {}
CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(Shape::CAPSULE_SHAPE),
_radius(radius), _halfHeight(halfHeight) {
updateBoundingRadius();
}
CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation) :
Shape(Shape::CAPSULE_SHAPE, position, rotation), _radius(radius), _halfHeight(halfHeight) {
updateBoundingRadius();
}
CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) :
Shape(Shape::CAPSULE_SHAPE), _radius(radius), _halfHeight(0.f) {
glm::vec3 axis = endPoint - startPoint;
float height = glm::length(axis);
if (height > EPSILON) {
_halfHeight = 0.5f * height;
axis /= height;
glm::vec3 yAxis(0.f, 1.f, 0.f);
float angle = glm::angle(axis, yAxis);
if (angle > EPSILON) {
axis = glm::normalize(glm::cross(yAxis, axis));
_rotation = glm::angleAxis(angle, axis);
}
}
updateBoundingRadius();
}
/// \param[out] startPoint is the center of start cap
void CapsuleShape::getStartPoint(glm::vec3& startPoint) const {
startPoint = getPosition() - _rotation * glm::vec3(0.f, _halfHeight, 0.f);
}
/// \param[out] endPoint is the center of the end cap
void CapsuleShape::getEndPoint(glm::vec3& endPoint) const {
endPoint = getPosition() + _rotation * glm::vec3(0.f, _halfHeight, 0.f);
}
void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
// default axis of a capsule is along the yAxis
axis = _rotation * glm::vec3(0.f, 1.f, 0.f);
}
void CapsuleShape::setRadius(float radius) {
_radius = radius;
updateBoundingRadius();
}
void CapsuleShape::setHalfHeight(float halfHeight) {
_halfHeight = halfHeight;
updateBoundingRadius();
}
void CapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) {
_radius = radius;
_halfHeight = halfHeight;
updateBoundingRadius();
}

46
libraries/shared/src/CapsuleShape.h Normal file
View file

@ -0,0 +1,46 @@
//
// CapsuleShape.h
// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __hifi__CapsuleShape__
#define __hifi__CapsuleShape__
#include "Shape.h"
// adebug bookmark TODO: convert to new world-frame approach
// default axis of CapsuleShape is Y-axis
class CapsuleShape : public Shape {
public:
CapsuleShape();
CapsuleShape(float radius, float halfHeight);
CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation);
CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint);
float getRadius() const { return _radius; }
float getHalfHeight() const { return _halfHeight; }
/// \param[out] startPoint is the center of start cap
void getStartPoint(glm::vec3& startPoint) const;
/// \param[out] endPoint is the center of the end cap
void getEndPoint(glm::vec3& endPoint) const;
void computeNormalizedAxis(glm::vec3& axis) const;
void setRadius(float radius);
void setHalfHeight(float height);
void setRadiusAndHalfHeight(float radius, float height);
protected:
void updateBoundingRadius() { _boundingRadius = _radius + _halfHeight; }
float _radius;
float _halfHeight;
};
#endif /* defined(__hifi__CapsuleShape__) */

7
libraries/shared/src/CollisionInfo.cpp
View file

@ -8,6 +8,7 @@
#include "CollisionInfo.h"
CollisionList::CollisionList(int maxSize) :
_maxSize(maxSize),
_size(0) {
@ -16,13 +17,17 @@ CollisionList::CollisionList(int maxSize) :
CollisionInfo* CollisionList::getNewCollision() {
// return pointer to existing CollisionInfo, or NULL of list is full
return (_size < _maxSize) ? &(_collisions[++_size]) : NULL;
return (_size < _maxSize) ? &(_collisions[_size++]) : NULL;
}
CollisionInfo* CollisionList::getCollision(int index) {
return (index > -1 && index < _size) ? &(_collisions[index]) : NULL;
}
CollisionInfo* CollisionList::getLastCollision() {
return (_size > 0) ? &(_collisions[_size - 1]) : NULL;
}
void CollisionList::clear() {
for (int i = 0; i < _size; ++i) {
// we only clear the important stuff

11
libraries/shared/src/CollisionInfo.h
View file

@ -10,6 +10,7 @@
#define __hifi__CollisionInfo__
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <QVector>
@ -79,6 +80,12 @@ public:
/// \return pointer to collision by index. NULL if index out of bounds.
CollisionInfo* getCollision(int index);
/// \return pointer to last collision on the list. NULL if list is empty
CollisionInfo* getLastCollision();
/// \return true if list is full
bool isFull() const { return _size == _maxSize; }
/// \return number of valid collisions
int size() const { return _size; }
@ -86,8 +93,8 @@ public:
void clear();
private:
int _maxSize;
int _size;
int _maxSize; // the container cannot get larger than this
int _size; // the current number of valid collisions in the list
QVector<CollisionInfo> _collisions;
};

88
libraries/shared/src/ListShape.cpp Normal file
View file

@ -0,0 +1,88 @@
//
// ListShape.cpp
//
// ListShape: A collection of shapes, each with a local transform.
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#include "ListShape.h"
// ListShapeEntry
void ListShapeEntry::updateTransform(const glm::vec3& rootPosition, const glm::quat& rootRotation) {
_shape->setPosition(rootPosition + rootRotation * _localPosition);
_shape->setRotation(_localRotation * rootRotation);
}
// ListShape
ListShape::~ListShape() {
clear();
}
void ListShape::setPosition(const glm::vec3& position) {
_subShapeTransformsAreDirty = true;
Shape::setPosition(position);
}
void ListShape::setRotation(const glm::quat& rotation) {
_subShapeTransformsAreDirty = true;
Shape::setRotation(rotation);
}
const Shape* ListShape::getSubShape(int index) const {
if (index < 0 || index > _subShapeEntries.size()) {
return NULL;
}
return _subShapeEntries[index]._shape;
}
void ListShape::updateSubTransforms() {
if (_subShapeTransformsAreDirty) {
for (int i = 0; i < _subShapeEntries.size(); ++i) {
_subShapeEntries[i].updateTransform(_position, _rotation);
}
_subShapeTransformsAreDirty = false;
}
}
void ListShape::addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation) {
if (shape) {
ListShapeEntry entry;
entry._shape = shape;
entry._localPosition = localPosition;
entry._localRotation = localRotation;
_subShapeEntries.push_back(entry);
}
}
void ListShape::setShapes(QVector<ListShapeEntry>& shapes) {
clear();
_subShapeEntries.swap(shapes);
// TODO: audit our new list of entries and delete any that have null pointers
computeBoundingRadius();
}
void ListShape::clear() {
// the ListShape owns its subShapes, so they need to be deleted
for (int i = 0; i < _subShapeEntries.size(); ++i) {
delete _subShapeEntries[i]._shape;
}
_subShapeEntries.clear();
setBoundingRadius(0.f);
}
void ListShape::computeBoundingRadius() {
float maxRadius = 0.f;
for (int i = 0; i < _subShapeEntries.size(); ++i) {
ListShapeEntry& entry = _subShapeEntries[i];
float radius = glm::length(entry._localPosition) + entry._shape->getBoundingRadius();
if (radius > maxRadius) {
maxRadius = radius;
}
}
setBoundingRadius(maxRadius);
}

65
libraries/shared/src/ListShape.h Normal file
View file

@ -0,0 +1,65 @@
//
// ListShape.h
//
// ListShape: A collection of shapes, each with a local transform.
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __hifi__ListShape__
#define __hifi__ListShape__
#include <QVector>
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/norm.hpp>
#include "Shape.h"
class ListShapeEntry {
public:
void updateTransform(const glm::vec3& position, const glm::quat& rotation);
Shape* _shape;
glm::vec3 _localPosition;
glm::quat _localRotation;
};
class ListShape : public Shape {
public:
ListShape() : Shape(LIST_SHAPE), _subShapeEntries(), _subShapeTransformsAreDirty(false) {}
ListShape(const glm::vec3& position, const glm::quat& rotation) :
Shape(LIST_SHAPE, position, rotation), _subShapeEntries(), _subShapeTransformsAreDirty(false) {}
~ListShape();
void setPosition(const glm::vec3& position);
void setRotation(const glm::quat& rotation);
const Shape* getSubShape(int index) const;
void updateSubTransforms();
int size() const { return _subShapeEntries.size(); }
void addShape(Shape* shape, const glm::vec3& localPosition, const glm::quat& localRotation);
void setShapes(QVector<ListShapeEntry>& shapes);
protected:
void clear();
void computeBoundingRadius();
QVector<ListShapeEntry> _subShapeEntries;
bool _subShapeTransformsAreDirty;
private:
ListShape(const ListShape& otherList); // don't implement this
};
#endif // __hifi__ListShape__

54
libraries/shared/src/Shape.h Normal file
View file

@ -0,0 +1,54 @@
//
// Shape.h
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __hifi__Shape__
#define __hifi__Shape__
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
class Shape {
public:
enum Type{
UNKNOWN_SHAPE = 0,
SPHERE_SHAPE,
CAPSULE_SHAPE,
BOX_SHAPE,
LIST_SHAPE
};
Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { }
virtual ~Shape() {}
int getType() const { return _type; }
float getBoundingRadius() const { return _boundingRadius; }
const glm::vec3& getPosition() const { return _position; }
const glm::quat& getRotation() const { return _rotation; }
virtual void setPosition(const glm::vec3& position) { _position = position; }
virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; }
protected:
// these ctors are protected (used by derived classes only)
Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {}
Shape(Type type, const glm::vec3& position)
: _type(type), _boundingRadius(0.f), _position(position), _rotation() {}
Shape(Type type, const glm::vec3& position, const glm::quat& rotation)
: _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {}
void setBoundingRadius(float radius) { _boundingRadius = radius; }
int _type;
float _boundingRadius;
glm::vec3 _position;
glm::quat _rotation;
};
#endif /* defined(__hifi__Shape__) */

408
libraries/shared/src/ShapeCollider.cpp Normal file
View file

@ -0,0 +1,408 @@
//
// ShapeCollider.cpp
// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#include <iostream>
#include <glm/gtx/norm.hpp>
#include "ShapeCollider.h"
// NOTE:
//
// * Large ListShape's are inefficient keep the lists short.
// * Collisions between lists of lists work in theory but are not recommended.
namespace ShapeCollider {
bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) {
// ATM we only have two shape types so we just check every case.
// TODO: make a fast lookup for correct method
int typeA = shapeA->getType();
int typeB = shapeB->getType();
if (typeA == Shape::SPHERE_SHAPE) {
const SphereShape* sphereA = static_cast<const SphereShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return sphereSphere(sphereA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return sphereCapsule(sphereA, static_cast<const CapsuleShape*>(shapeB), collisions);
}
} else if (typeA == Shape::CAPSULE_SHAPE) {
const CapsuleShape* capsuleA = static_cast<const CapsuleShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return capsuleSphere(capsuleA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(shapeB), collisions);
}
} else if (typeA == Shape::LIST_SHAPE) {
const ListShape* listA = static_cast<const ListShape*>(shapeA);
if (typeB == Shape::SPHERE_SHAPE) {
return listSphere(listA, static_cast<const SphereShape*>(shapeB), collisions);
} else if (typeB == Shape::CAPSULE_SHAPE) {
return listCapsule(listA, static_cast<const CapsuleShape*>(shapeB), collisions);
}
}
return false;
}
bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) {
glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition();
float distanceSquared = glm::dot(BA, BA);
float totalRadius = sphereA->getRadius() + sphereB->getRadius();
if (distanceSquared < totalRadius * totalRadius) {
// normalize BA
float distance = sqrtf(distanceSquared);
if (distance < EPSILON) {
// the spheres are on top of each other, so we pick an arbitrary penetration direction
BA = glm::vec3(0.f, 1.f, 0.f);
distance = totalRadius;
} else {
BA /= distance;
}
// penetration points from A into B
CollisionInfo* collision = collisions.getNewCollision();
if (collision) {
collision->_penetration = BA * (totalRadius - distance);
// contactPoint is on surface of A
collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA;
return true;
}
}
return false;
}
bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) {
// find sphereA's closest approach to axis of capsuleB
glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition();
glm::vec3 capsuleAxis;
capsuleB->computeNormalizedAxis(capsuleAxis);
float axialDistance = - glm::dot(BA, capsuleAxis);
float absAxialDistance = fabs(axialDistance);
float totalRadius = sphereA->getRadius() + capsuleB->getRadius();
if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) {
glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B
float radialDistance2 = glm::length2(radialAxis);
if (radialDistance2 > totalRadius * totalRadius) {
// sphere is too far from capsule axis
return false;
}
if (absAxialDistance > capsuleB->getHalfHeight()) {
// sphere hits capsule on a cap --> recompute radialAxis to point from spherA to cap center
float sign = (axialDistance > 0.f) ? 1.f : -1.f;
radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis;
radialDistance2 = glm::length2(radialAxis);
}
if (radialDistance2 > EPSILON * EPSILON) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// normalize the radialAxis
float radialDistance = sqrtf(radialDistance2);
radialAxis /= radialDistance;
// penetration points from A into B
collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B
// contactPoint is on surface of sphereA
collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis;
} else {
// A is on B's axis, so the penetration is undefined...
if (absAxialDistance > capsuleB->getHalfHeight()) {
// ...for the cylinder case (for now we pretend the collision doesn't exist)
return false;
}
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// ... but still defined for the cap case
if (axialDistance < 0.f) {
// we're hitting the start cap, so we negate the capsuleAxis
capsuleAxis *= -1;
}
// penetration points from A into B
float sign = (axialDistance > 0.f) ? -1.f : 1.f;
collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis;
// contactPoint is on surface of sphereA
collision->_contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis;
}
return true;
}
return false;
}
bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) {
// find sphereB's closest approach to axis of capsuleA
glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition();
glm::vec3 capsuleAxis;
capsuleA->computeNormalizedAxis(capsuleAxis);
float axialDistance = - glm::dot(AB, capsuleAxis);
float absAxialDistance = fabs(axialDistance);
float totalRadius = sphereB->getRadius() + capsuleA->getRadius();
if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) {
glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA
float radialDistance2 = glm::length2(radialAxis);
if (radialDistance2 > totalRadius * totalRadius) {
// sphere is too far from capsule axis
return false;
}
// closestApproach = point on capsuleA's axis that is closest to sphereB's center
glm::vec3 closestApproach = capsuleA->getPosition() + axialDistance * capsuleAxis;
if (absAxialDistance > capsuleA->getHalfHeight()) {
// sphere hits capsule on a cap
// --> recompute radialAxis and closestApproach
float sign = (axialDistance > 0.f) ? 1.f : -1.f;
closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis;
radialAxis = closestApproach - sphereB->getPosition();
radialDistance2 = glm::length2(radialAxis);
}
if (radialDistance2 > EPSILON * EPSILON) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// normalize the radialAxis
float radialDistance = sqrtf(radialDistance2);
radialAxis /= radialDistance;
// penetration points from A into B
collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B
// contactPoint is on surface of capsuleA
collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis;
} else {
// A is on B's axis, so the penetration is undefined...
if (absAxialDistance > capsuleA->getHalfHeight()) {
// ...for the cylinder case (for now we pretend the collision doesn't exist)
return false;
} else {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// ... but still defined for the cap case
if (axialDistance < 0.f) {
// we're hitting the start cap, so we negate the capsuleAxis
capsuleAxis *= -1;
}
float sign = (axialDistance > 0.f) ? 1.f : -1.f;
collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis;
// contactPoint is on surface of sphereA
collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis;
}
}
return true;
}
return false;
}
bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions) {
glm::vec3 axisA;
capsuleA->computeNormalizedAxis(axisA);
glm::vec3 axisB;
capsuleB->computeNormalizedAxis(axisB);
glm::vec3 centerA = capsuleA->getPosition();
glm::vec3 centerB = capsuleB->getPosition();
// NOTE: The formula for closest approach between two lines is:
// d = [(B - A) . (a - (a.b)b)] / (1 - (a.b)^2)
float aDotB = glm::dot(axisA, axisB);
float denominator = 1.f - aDotB * aDotB;
float totalRadius = capsuleA->getRadius() + capsuleB->getRadius();
if (denominator > EPSILON) {
// distances to points of closest approach
float distanceA = glm::dot((centerB - centerA), (axisA - (aDotB) * axisB)) / denominator;
float distanceB = glm::dot((centerA - centerB), (axisB - (aDotB) * axisA)) / denominator;
// clamp the distances to the ends of the capsule line segments
float absDistanceA = fabs(distanceA);
if (absDistanceA > capsuleA->getHalfHeight() + capsuleA->getRadius()) {
float signA = distanceA < 0.f ? -1.f : 1.f;
distanceA = signA * capsuleA->getHalfHeight();
}
float absDistanceB = fabs(distanceB);
if (absDistanceB > capsuleB->getHalfHeight() + capsuleB->getRadius()) {
float signB = distanceB < 0.f ? -1.f : 1.f;
distanceB = signB * capsuleB->getHalfHeight();
}
// collide like spheres at closest approaches (do most of the math relative to B)
glm::vec3 BA = (centerB + distanceB * axisB) - (centerA + distanceA * axisA);
float distanceSquared = glm::dot(BA, BA);
if (distanceSquared < totalRadius * totalRadius) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// normalize BA
float distance = sqrtf(distanceSquared);
if (distance < EPSILON) {
// the contact spheres are on top of each other, so we need to pick a penetration direction...
// try vector between the capsule centers...
BA = centerB - centerA;
distanceSquared = glm::length2(BA);
if (distanceSquared > EPSILON * EPSILON) {
distance = sqrtf(distanceSquared);
BA /= distance;
} else
{
// the capsule centers are on top of each other!
// give up on a valid penetration direction and just use the yAxis
BA = glm::vec3(0.f, 1.f, 0.f);
distance = glm::max(capsuleB->getRadius(), capsuleA->getRadius());
}
} else {
BA /= distance;
}
// penetration points from A into B
collision->_penetration = BA * (totalRadius - distance);
// contactPoint is on surface of A
collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA;
return true;
}
} else {
// capsules are approximiately parallel but might still collide
glm::vec3 BA = centerB - centerA;
float axialDistance = glm::dot(BA, axisB);
if (axialDistance > totalRadius + capsuleA->getHalfHeight() + capsuleB->getHalfHeight()) {
return false;
}
BA = BA - axialDistance * axisB; // BA now points from centerA to axisB (perp to axis)
float distanceSquared = glm::length2(BA);
if (distanceSquared < totalRadius * totalRadius) {
CollisionInfo* collision = collisions.getNewCollision();
if (!collision) {
// collisions list is full
return false;
}
// We have all the info we need to compute the penetration vector...
// normalize BA
float distance = sqrtf(distanceSquared);
if (distance < EPSILON) {
// the spheres are on top of each other, so we pick an arbitrary penetration direction
BA = glm::vec3(0.f, 1.f, 0.f);
} else {
BA /= distance;
}
// penetration points from A into B
collision->_penetration = BA * (totalRadius - distance);
// However we need some more world-frame info to compute the contactPoint,
// which is on the surface of capsuleA...
//
// Find the overlapping secion of the capsules --> they collide as if there were
// two spheres at the midpoint of this overlapping section.
// So we project all endpoints to axisB, find the interior pair,
// and put A's proxy sphere on axisA at the midpoint of this section.
// sort the projections as much as possible during calculation
float points[5];
points[0] = -capsuleB->getHalfHeight();
points[1] = axialDistance - capsuleA->getHalfHeight();
points[2] = axialDistance + capsuleA->getHalfHeight();
points[3] = capsuleB->getHalfHeight();
// Since there are only three comparisons to do we unroll the sort algorithm...
// and use a fifth slot as temp during swap.
if (points[4] > points[2]) {
points[4] = points[1];
points[1] = points[2];
points[2] = points[4];
}
if (points[2] > points[3]) {
points[4] = points[2];
points[2] = points[3];
points[3] = points[4];
}
if (points[0] > points[1]) {
points[4] = points[0];
points[0] = points[1];
points[1] = points[4];
}
// average the internal pair, and then do the math from centerB
collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB
+ (capsuleA->getRadius() - distance) * BA;
return true;
}
}
return false;
}
bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listB->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereSphere(sphereA, static_cast<const SphereShape*>(subShape), collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = sphereCapsule(sphereA, static_cast<const CapsuleShape*>(subShape), collisions) || touching;
}
}
return touching;
}
bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listB->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listB->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = capsuleSphere(capsuleA, static_cast<const SphereShape*>(subShape), collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleCapsule(capsuleA, static_cast<const CapsuleShape*>(subShape), collisions) || touching;
}
}
return touching;
}
bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereSphere(static_cast<const SphereShape*>(subShape), sphereB, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleSphere(static_cast<const CapsuleShape*>(subShape), sphereB, collisions) || touching;
}
}
return touching;
}
bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
int subType = subShape->getType();
if (subType == Shape::SPHERE_SHAPE) {
touching = sphereCapsule(static_cast<const SphereShape*>(subShape), capsuleB, collisions) || touching;
} else if (subType == Shape::CAPSULE_SHAPE) {
touching = capsuleCapsule(static_cast<const CapsuleShape*>(subShape), capsuleB, collisions) || touching;
}
}
return touching;
}
bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions) {
bool touching = false;
for (int i = 0; i < listA->size() && !collisions.isFull(); ++i) {
const Shape* subShape = listA->getSubShape(i);
for (int j = 0; j < listB->size() && !collisions.isFull(); ++j) {
touching = shapeShape(subShape, listB->getSubShape(j), collisions) || touching;
}
}
return touching;
}
} // namespace ShapeCollider

82
libraries/shared/src/ShapeCollider.h Normal file
View file

@ -0,0 +1,82 @@
//
// ShapeCollider.h
// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __hifi__ShapeCollider__
#define __hifi__ShapeCollider__
#include "CapsuleShape.h"
#include "CollisionInfo.h"
#include "ListShape.h"
#include "SharedUtil.h"
#include "SphereShape.h"
namespace ShapeCollider {
/// \param shapeA pointer to first shape
/// \param shapeB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool shapeShape(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions);
/// \param sphereA pointer to first shape
/// \param sphereB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions);
/// \param sphereA pointer to first shape
/// \param capsuleB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions);
/// \param capsuleA pointer to first shape
/// \param sphereB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions);
/// \param capsuleA pointer to first shape
/// \param capsuleB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, CollisionList& collisions);
/// \param sphereA pointer to first shape
/// \param listB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool sphereList(const SphereShape* sphereA, const ListShape* listB, CollisionList& collisions);
/// \param capuleA pointer to first shape
/// \param listB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool capsuleList(const CapsuleShape* capsuleA, const ListShape* listB, CollisionList& collisions);
/// \param listA pointer to first shape
/// \param sphereB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listSphere(const ListShape* listA, const SphereShape* sphereB, CollisionList& collisions);
/// \param listA pointer to first shape
/// \param capsuleB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listCapsule(const ListShape* listA, const CapsuleShape* capsuleB, CollisionList& collisions);
/// \param listA pointer to first shape
/// \param capsuleB pointer to second shape
/// \param[out] collisions where to append collision details
/// \return true if shapes collide
bool listList(const ListShape* listA, const ListShape* listB, CollisionList& collisions);
} // namespace ShapeCollider
#endif // __hifi__ShapeCollider__

31
libraries/shared/src/SphereShape.h Normal file
View file

@ -0,0 +1,31 @@
//
// SphereShape.h
// hifi
//
// Created by Andrew Meadows on 2014.02.20
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __hifi__SphereShape__
#define __hifi__SphereShape__
#include "Shape.h"
class SphereShape : public Shape {
public:
SphereShape() : Shape(Shape::SPHERE_SHAPE) {}
SphereShape(float radius) : Shape(Shape::SPHERE_SHAPE) {
_boundingRadius = radius;
}
SphereShape(float radius, const glm::vec3& position) : Shape(Shape::SPHERE_SHAPE, position) {
_boundingRadius = radius;
}
float getRadius() const { return _boundingRadius; }
void setRadius(float radius) { _boundingRadius = radius; }
};
#endif /* defined(__hifi__SphereShape__) */

34
tests/physics/CMakeLists.txt Normal file
View file

@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 2.8)
set(TARGET_NAME physics-tests)
set(ROOT_DIR ../..)
set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
# setup for find modules
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../../cmake/modules/")
#find_package(Qt5Network REQUIRED)
#find_package(Qt5Script REQUIRED)
#find_package(Qt5Widgets REQUIRED)
include(${MACRO_DIR}/SetupHifiProject.cmake)
setup_hifi_project(${TARGET_NAME} TRUE)
include(${MACRO_DIR}/AutoMTC.cmake)
auto_mtc(${TARGET_NAME} ${ROOT_DIR})
#qt5_use_modules(${TARGET_NAME} Network Script Widgets)
#include glm
include(${MACRO_DIR}/IncludeGLM.cmake)
include_glm(${TARGET_NAME} ${ROOT_DIR})
# link in the shared libraries
include(${MACRO_DIR}/LinkHifiLibrary.cmake)
link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
IF (WIN32)
#target_link_libraries(${TARGET_NAME} Winmm Ws2_32)
ENDIF(WIN32)

104
tests/physics/src/CollisionInfoTests.cpp Normal file
View file

@ -0,0 +1,104 @@
//
// CollisionInfoTests.cpp
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#include <iostream>
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <CollisionInfo.h>
#include <SharedUtil.h>
#include "CollisionInfoTests.h"
#include "PhysicsTestUtil.h"
/*
void CollisionInfoTests::rotateThenTranslate() {
CollisionInfo collision;
collision._penetration = xAxis;
collision._contactPoint = yAxis;
collision._addedVelocity = xAxis + yAxis + zAxis;
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
float distance = 3.f;
glm::vec3 translation = distance * yAxis;
collision.rotateThenTranslate(rotation, translation);
float error = glm::distance(collision._penetration, yAxis);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._penetration = " << collision._penetration
<< " but we expected " << yAxis
<< std::endl;
}
glm::vec3 expectedContactPoint = -xAxis + translation;
error = glm::distance(collision._contactPoint, expectedContactPoint);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._contactPoint = " << collision._contactPoint
<< " but we expected " << expectedContactPoint
<< std::endl;
}
glm::vec3 expectedAddedVelocity = yAxis - xAxis + zAxis;
error = glm::distance(collision._addedVelocity, expectedAddedVelocity);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._addedVelocity = " << collision._contactPoint
<< " but we expected " << expectedAddedVelocity
<< std::endl;
}
}
void CollisionInfoTests::translateThenRotate() {
CollisionInfo collision;
collision._penetration = xAxis;
collision._contactPoint = yAxis;
collision._addedVelocity = xAxis + yAxis + zAxis;
glm::quat rotation = glm::angleAxis( -rightAngle, zAxis);
float distance = 3.f;
glm::vec3 translation = distance * yAxis;
collision.translateThenRotate(translation, rotation);
float error = glm::distance(collision._penetration, -yAxis);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._penetration = " << collision._penetration
<< " but we expected " << -yAxis
<< std::endl;
}
glm::vec3 expectedContactPoint = (1.f + distance) * xAxis;
error = glm::distance(collision._contactPoint, expectedContactPoint);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._contactPoint = " << collision._contactPoint
<< " but we expected " << expectedContactPoint
<< std::endl;
}
glm::vec3 expectedAddedVelocity = - yAxis + xAxis + zAxis;
error = glm::distance(collision._addedVelocity, expectedAddedVelocity);
if (error > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: collision._addedVelocity = " << collision._contactPoint
<< " but we expected " << expectedAddedVelocity
<< std::endl;
}
}
*/
void CollisionInfoTests::runAllTests() {
// CollisionInfoTests::rotateThenTranslate();
// CollisionInfoTests::translateThenRotate();
}

21
tests/physics/src/CollisionInfoTests.h Normal file
View file

@ -0,0 +1,21 @@
//
// CollisionInfoTests.h
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __tests__CollisionInfoTests__
#define __tests__CollisionInfoTests__
namespace CollisionInfoTests {
// void rotateThenTranslate();
// void translateThenRotate();
void runAllTests();
}
#endif // __tests__CollisionInfoTests__

37
tests/physics/src/PhysicsTestUtil.cpp Normal file
View file

@ -0,0 +1,37 @@
//
// PhysicsTestUtil.cpp
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#include <glm/gtc/type_ptr.hpp>
#include "PhysicsTestUtil.h"
std::ostream& operator<<(std::ostream& s, const glm::vec3& v) {
s << "<" << v.x << "," << v.y << "," << v.z << ">";
return s;
}
std::ostream& operator<<(std::ostream& s, const glm::quat& q) {
s << "<" << q.x << "," << q.y << "," << q.z << "," << q.w << ">";
return s;
}
std::ostream& operator<<(std::ostream& s, const glm::mat4& m) {
s << "[";
for (int j = 0; j < 4; ++j) {
s << " " << m[0][j] << " " << m[1][j] << " " << m[2][j] << " " << m[3][j] << ";";
}
s << " ]";
return s;
}
std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) {
s << "[penetration=" << c._penetration
<< ", contactPoint=" << c._contactPoint
<< ", addedVelocity=" << c._addedVelocity;
return s;
}

28
tests/physics/src/PhysicsTestUtil.h Normal file
View file

@ -0,0 +1,28 @@
//
// PhysicsTestUtil.h
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __tests__PhysicsTestUtil__
#define __tests__PhysicsTestUtil__
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <CollisionInfo.h>
const glm::vec3 xAxis(1.f, 0.f, 0.f);
const glm::vec3 yAxis(0.f, 1.f, 0.f);
const glm::vec3 zAxis(0.f, 0.f, 1.f);
const float rightAngle = 90.f; // degrees
std::ostream& operator<<(std::ostream& s, const glm::vec3& v);
std::ostream& operator<<(std::ostream& s, const glm::quat& q);
std::ostream& operator<<(std::ostream& s, const glm::mat4& m);
std::ostream& operator<<(std::ostream& s, const CollisionInfo& c);
#endif // __tests__PhysicsTestUtil__

706
tests/physics/src/ShapeColliderTests.cpp Normal file
View file

@ -0,0 +1,706 @@
//
// ShapeColliderTests.cpp
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
//#include <stdio.h>
#include <iostream>
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <CollisionInfo.h>
#include <ShapeCollider.h>
#include <SharedUtil.h>
#include <SphereShape.h>
#include "PhysicsTestUtil.h"
#include "ShapeColliderTests.h"
const glm::vec3 origin(0.f);
void ShapeColliderTests::sphereMissesSphere() {
// non-overlapping spheres of unequal size
float radiusA = 7.f;
float radiusB = 3.f;
float alpha = 1.2f;
float beta = 1.3f;
glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
float offsetDistance = alpha * radiusA + beta * radiusB;
SphereShape sphereA(radiusA, origin);
SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
CollisionList collisions(16);
// collide A to B...
{
bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
// collide B to A...
{
bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
// also test shapeShape
{
bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size()
<< std::endl;
}
}
void ShapeColliderTests::sphereTouchesSphere() {
// overlapping spheres of unequal size
float radiusA = 7.f;
float radiusB = 3.f;
float alpha = 0.2f;
float beta = 0.3f;
glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.f, 2.f, 3.f));
float offsetDistance = alpha * radiusA + beta * radiusB;
float expectedPenetrationDistance = (1.f - alpha) * radiusA + (1.f - beta) * radiusB;
glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
SphereShape sphereA(radiusA, origin);
SphereShape sphereB(radiusB, offsetDistance * offsetDirection);
CollisionList collisions(16);
int numCollisions = 0;
// collide A to B...
{
bool touching = ShapeCollider::shapeShape(&sphereA, &sphereB, collisions);
if (!touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should touch" << std::endl;
} else {
++numCollisions;
}
// verify state of collisions
if (numCollisions != collisions.size()) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size()
<< std::endl;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
if (!collision) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: null collision" << std::endl;
}
// penetration points from sphereA into sphereB
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition();
glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB);
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
// collide B to A...
{
bool touching = ShapeCollider::shapeShape(&sphereB, &sphereA, collisions);
if (!touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into sphereB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
float inaccuracy = glm::length(collision->_penetration + expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition();
glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA);
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
}
void ShapeColliderTests::sphereMissesCapsule() {
// non-overlapping sphere and capsule
float radiusA = 1.5f;
float radiusB = 2.3f;
float totalRadius = radiusA + radiusB;
float halfHeightB = 1.7f;
float axialOffset = totalRadius + 1.1f * halfHeightB;
float radialOffset = 1.2f * radiusA + 1.3f * radiusB;
SphereShape sphereA(radiusA);
CapsuleShape capsuleB(radiusB, halfHeightB);
// give the capsule some arbirary transform
float angle = 37.8;
glm::vec3 axis = glm::normalize( glm::vec3(-7.f, 2.8f, 9.3f) );
glm::quat rotation = glm::angleAxis(angle, axis);
glm::vec3 translation(15.1f, -27.1f, -38.6f);
capsuleB.setRotation(rotation);
capsuleB.setPosition(translation);
CollisionList collisions(16);
// walk sphereA along the local yAxis next to, but not touching, capsuleB
glm::vec3 localStartPosition(radialOffset, axialOffset, 0.f);
int numberOfSteps = 10;
float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1);
for (int i = 0; i < numberOfSteps; ++i) {
// translate sphereA into world-frame
glm::vec3 localPosition = localStartPosition + (float(i) * delta) * yAxis;
sphereA.setPosition(rotation * localPosition + translation);
// sphereA agains capsuleB
if (ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch"
<< std::endl;
}
// capsuleB against sphereA
if (ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch"
<< std::endl;
}
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size()
<< std::endl;
}
}
void ShapeColliderTests::sphereTouchesCapsule() {
// overlapping sphere and capsule
float radiusA = 2.f;
float radiusB = 1.f;
float totalRadius = radiusA + radiusB;
float halfHeightB = 2.f;
float alpha = 0.5f;
float beta = 0.5f;
float radialOffset = alpha * radiusA + beta * radiusB;
SphereShape sphereA(radiusA);
CapsuleShape capsuleB(radiusB, halfHeightB);
CollisionList collisions(16);
int numCollisions = 0;
{ // sphereA collides with capsuleB's cylindrical wall
sphereA.setPosition(radialOffset * xAxis);
if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - (radialOffset - totalRadius) * xAxis;
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition();
glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis;
expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach);
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
{ // sphereA hits end cap at axis
glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setPosition(axialOffset * yAxis);
if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 endPoint;
capsuleB.getEndPoint(endPoint);
expectedContactPoint = endPoint + radiusB * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
{ // sphereA hits start cap at axis
glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setPosition(axialOffset * yAxis);
if (!ShapeCollider::shapeShape(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::shapeShape(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch"
<< std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - ((1.f - alpha) * radiusA + (1.f - beta) * radiusB) * yAxis;
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 startPoint;
capsuleB.getStartPoint(startPoint);
expectedContactPoint = startPoint - radiusB * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
if (collisions.size() != numCollisions) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size()
<< std::endl;
}
}
void ShapeColliderTests::capsuleMissesCapsule() {
// non-overlapping capsules
float radiusA = 2.f;
float halfHeightA = 3.f;
float radiusB = 3.f;
float halfHeightB = 4.f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
CapsuleShape capsuleA(radiusA, halfHeightA);
CapsuleShape capsuleB(radiusA, halfHeightA);
CollisionList collisions(16);
// side by side
capsuleB.setPosition((1.01f * totalRadius) * xAxis);
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
// end to end
capsuleB.setPosition((1.01f * totalHalfLength) * xAxis);
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
// rotate B and move it to the side
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch"
<< std::endl;
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size()
<< std::endl;
}
}
void ShapeColliderTests::capsuleTouchesCapsule() {
// overlapping capsules
float radiusA = 2.f;
float halfHeightA = 3.f;
float radiusB = 3.f;
float halfHeightB = 4.f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
CapsuleShape capsuleA(radiusA, halfHeightA);
CapsuleShape capsuleB(radiusB, halfHeightB);
CollisionList collisions(16);
int numCollisions = 0;
{ // side by side
capsuleB.setPosition((0.99f * totalRadius) * xAxis);
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
}
{ // end to end
capsuleB.setPosition((0.99f * totalHalfLength) * yAxis);
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
}
{ // rotate B and move it to the side
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
}
{ // again, but this time check collision details
float overlap = 0.1f;
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis;
capsuleB.setPosition(positionB);
// capsuleA vs capsuleB
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = overlap * xAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
// capsuleB vs capsuleA
if (!ShapeCollider::shapeShape(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - overlap * xAxis;
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
expectedContactPoint = capsuleB.getPosition() - (radiusB + halfHeightB) * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
{ // collide cylinder wall against cylinder wall
float overlap = 0.137f;
float shift = 0.317f * halfHeightA;
glm::quat rotation = glm::angleAxis(rightAngle, zAxis);
capsuleB.setRotation(rotation);
glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis;
capsuleB.setPosition(positionB);
// capsuleA vs capsuleB
if (!ShapeCollider::shapeShape(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch"
<< std::endl;
} else {
++numCollisions;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = overlap * zAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration
<< std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * zAxis + shift * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint
<< std::endl;
}
}
}
void ShapeColliderTests::runAllTests() {
sphereMissesSphere();
sphereTouchesSphere();
sphereMissesCapsule();
sphereTouchesCapsule();
capsuleMissesCapsule();
capsuleTouchesCapsule();
}

26
tests/physics/src/ShapeColliderTests.h Normal file
View file

@ -0,0 +1,26 @@
//
// ShapeColliderTests.h
// physics-tests
//
// Created by Andrew Meadows on 2014.02.21
// Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//
#ifndef __tests__ShapeColliderTests__
#define __tests__ShapeColliderTests__
namespace ShapeColliderTests {
void sphereMissesSphere();
void sphereTouchesSphere();
void sphereMissesCapsule();
void sphereTouchesCapsule();
void capsuleMissesCapsule();
void capsuleTouchesCapsule();
void runAllTests();
}
#endif // __tests__ShapeColliderTests__

11
tests/physics/src/main.cpp Normal file
View file

@ -0,0 +1,11 @@
//
// main.cpp
// physics-tests
//
#include "ShapeColliderTests.h"
int main(int argc, char** argv) {
ShapeColliderTests::runAllTests();
return 0;
}