diff --git a/assignment-client/src/AssignmentClient.cpp b/assignment-client/src/AssignmentClient.cpp index cc4292ad15..f3458c8afb 100644 --- a/assignment-client/src/AssignmentClient.cpp +++ b/assignment-client/src/AssignmentClient.cpp @@ -24,12 +24,8 @@ const long long ASSIGNMENT_REQUEST_INTERVAL_MSECS = 1 * 1000; int hifiSockAddrMeta = qRegisterMetaType("HifiSockAddr"); -AssignmentClient::AssignmentClient(int &argc, char **argv, - Assignment::Type requestAssignmentType, - const HifiSockAddr& customAssignmentServerSocket, - const char* requestAssignmentPool) : +AssignmentClient::AssignmentClient(int &argc, char **argv) : QCoreApplication(argc, argv), - _requestAssignment(Assignment::RequestCommand, requestAssignmentType, requestAssignmentPool), _currentAssignment(NULL) { // register meta type is required for queued invoke method on Assignment subclasses @@ -37,12 +33,53 @@ AssignmentClient::AssignmentClient(int &argc, char **argv, // set the logging target to the the CHILD_TARGET_NAME Logging::setTargetName(ASSIGNMENT_CLIENT_TARGET_NAME); + const char ASSIGNMENT_TYPE_OVVERIDE_OPTION[] = "-t"; + const char* assignmentTypeString = getCmdOption(argc, (const char**)argv, ASSIGNMENT_TYPE_OVVERIDE_OPTION); + + Assignment::Type requestAssignmentType = Assignment::AllTypes; + + if (assignmentTypeString) { + // the user is asking to only be assigned to a particular type of assignment + // so set that as the ::overridenAssignmentType to be used in requests + requestAssignmentType = (Assignment::Type) atoi(assignmentTypeString); + } + + const char ASSIGNMENT_POOL_OPTION[] = "--pool"; + const char* requestAssignmentPool = getCmdOption(argc, (const char**) argv, ASSIGNMENT_POOL_OPTION); + + + // setup our _requestAssignment member variable from the passed arguments + _requestAssignment = Assignment(Assignment::RequestCommand, requestAssignmentType, requestAssignmentPool); + // create a NodeList as an unassigned client NodeList* nodeList = NodeList::createInstance(NODE_TYPE_UNASSIGNED); + const char CUSTOM_ASSIGNMENT_SERVER_HOSTNAME_OPTION[] = "-a"; + const char CUSTOM_ASSIGNMENT_SERVER_PORT_OPTION[] = "-p"; + + // grab the overriden assignment-server hostname from argv, if it exists + const char* customAssignmentServerHostname = getCmdOption(argc, (const char**)argv, CUSTOM_ASSIGNMENT_SERVER_HOSTNAME_OPTION); + const char* customAssignmentServerPortString = getCmdOption(argc,(const char**)argv, CUSTOM_ASSIGNMENT_SERVER_PORT_OPTION); + + HifiSockAddr customAssignmentSocket; + + if (customAssignmentServerHostname || customAssignmentServerPortString) { + + // set the custom port or default if it wasn't passed + unsigned short assignmentServerPort = customAssignmentServerPortString + ? atoi(customAssignmentServerPortString) : DEFAULT_DOMAIN_SERVER_PORT; + + // set the custom hostname or default if it wasn't passed + if (!customAssignmentServerHostname) { + customAssignmentServerHostname = DEFAULT_ASSIGNMENT_SERVER_HOSTNAME; + } + + customAssignmentSocket = HifiSockAddr(customAssignmentServerHostname, assignmentServerPort); + } + // set the custom assignment socket if we have it - if (!customAssignmentServerSocket.isNull()) { - nodeList->setAssignmentServerSocket(customAssignmentServerSocket); + if (!customAssignmentSocket.isNull()) { + nodeList->setAssignmentServerSocket(customAssignmentSocket); } // call a timer function every ASSIGNMENT_REQUEST_INTERVAL_MSECS to ask for assignment, if required diff --git a/assignment-client/src/AssignmentClient.h b/assignment-client/src/AssignmentClient.h index 48311b0275..f19c8015b3 100644 --- a/assignment-client/src/AssignmentClient.h +++ b/assignment-client/src/AssignmentClient.h @@ -16,10 +16,7 @@ class AssignmentClient : public QCoreApplication { Q_OBJECT public: - AssignmentClient(int &argc, char **argv, - Assignment::Type requestAssignmentType = Assignment::AllTypes, - const HifiSockAddr& customAssignmentServerSocket = HifiSockAddr(), - const char* requestAssignmentPool = NULL); + AssignmentClient(int &argc, char **argv); private slots: void sendAssignmentRequest(); void readPendingDatagrams(); diff --git a/assignment-client/src/AssignmentClientMonitor.cpp b/assignment-client/src/AssignmentClientMonitor.cpp new file mode 100644 index 0000000000..fa050a0dac --- /dev/null +++ b/assignment-client/src/AssignmentClientMonitor.cpp @@ -0,0 +1,56 @@ +// +// AssignmentClientMonitor.cpp +// hifi +// +// Created by Stephen Birarda on 1/10/2014. +// Copyright (c) 2014 HighFidelity, Inc. All rights reserved. +// + +#include + +#include "AssignmentClientMonitor.h" + +const char* NUM_FORKS_PARAMETER = "-n"; + +const char ASSIGNMENT_CLIENT_MONITOR_TARGET_NAME[] = "assignment-client-monitor"; + +AssignmentClientMonitor::AssignmentClientMonitor(int &argc, char **argv, int numAssignmentClientForks) : + QCoreApplication(argc, argv) +{ + // start the Logging class with the parent's target name + Logging::setTargetName(ASSIGNMENT_CLIENT_MONITOR_TARGET_NAME); + + _childArguments = arguments(); + + // remove the parameter for the number of forks so it isn't passed to the child forked processes + int forksParameterIndex = _childArguments.indexOf(NUM_FORKS_PARAMETER); + + // this removes both the "-n" parameter and the number of forks passed + _childArguments.removeAt(forksParameterIndex); + _childArguments.removeAt(forksParameterIndex); + + // use QProcess to fork off a process for each of the child assignment clients + for (int i = 0; i < numAssignmentClientForks; i++) { + spawnChildClient(); + } +} + +void AssignmentClientMonitor::spawnChildClient() { + QProcess *assignmentClient = new QProcess(this); + + // make sure that the output from the child process appears in our output + assignmentClient->setProcessChannelMode(QProcess::ForwardedChannels); + + assignmentClient->start(applicationFilePath(), _childArguments); + + // link the child processes' finished slot to our childProcessFinished slot + connect(assignmentClient, SIGNAL(finished(int, QProcess::ExitStatus)), this, + SLOT(childProcessFinished(int, QProcess::ExitStatus))); + + qDebug() << "Spawned a child client with PID" << assignmentClient->pid() << "\n"; +} + +void AssignmentClientMonitor::childProcessFinished(int exitCode, QProcess::ExitStatus exitStatus) { + qDebug() << "Replacing dead child assignment client with a new one.\n"; + spawnChildClient(); +} \ No newline at end of file diff --git a/assignment-client/src/AssignmentClientMonitor.h b/assignment-client/src/AssignmentClientMonitor.h new file mode 100644 index 0000000000..259a6d6db7 --- /dev/null +++ b/assignment-client/src/AssignmentClientMonitor.h @@ -0,0 +1,31 @@ +// +// AssignmentClientMonitor.h +// hifi +// +// Created by Stephen Birarda on 1/10/2014. +// Copyright (c) 2014 HighFidelity, Inc. All rights reserved. +// + +#ifndef __hifi__AssignmentClientMonitor__ +#define __hifi__AssignmentClientMonitor__ + +#include +#include + +#include + +extern const char* NUM_FORKS_PARAMETER; + +class AssignmentClientMonitor : public QCoreApplication { + Q_OBJECT +public: + AssignmentClientMonitor(int &argc, char **argv, int numAssignmentClientForks); +private slots: + void childProcessFinished(int exitCode, QProcess::ExitStatus exitStatus); +private: + void spawnChildClient(); + + QStringList _childArguments; +}; + +#endif /* defined(__hifi__AssignmentClientMonitor__) */ diff --git a/assignment-client/src/main.cpp b/assignment-client/src/main.cpp index 28516b37b8..7ee0ea9e14 100644 --- a/assignment-client/src/main.cpp +++ b/assignment-client/src/main.cpp @@ -6,172 +6,33 @@ // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. // -#include -#include -#include -#include -#include - #include -#include -#include #include -#include -#include "Agent.h" #include "Assignment.h" #include "AssignmentClient.h" -#include "audio/AudioMixer.h" -#include "avatars/AvatarMixer.h" - -const char PARENT_TARGET_NAME[] = "assignment-client-monitor"; - -pid_t* childForks = NULL; -HifiSockAddr customAssignmentSocket; -int numForks = 0; -Assignment::Type overiddenAssignmentType = Assignment::AllTypes; -const char* assignmentPool = NULL; - -int argc = 0; -char** argv = NULL; - -int childClient() { - AssignmentClient client(::argc, ::argv, ::overiddenAssignmentType, customAssignmentSocket, ::assignmentPool); - return client.exec(); -} - -void sigchldHandler(int sig) { - pid_t processID; - int status; - - while ((processID = waitpid(-1, &status, WNOHANG)) != -1) { - if (processID == 0) { - // there are no more children to process, break out of here - break; - } - - int newForkProcessID = 0; - - // find the dead process in the array of child forks - for (int i = 0; i < ::numForks; i++) { - if (::childForks[i] == processID) { - - newForkProcessID = fork(); - if (newForkProcessID == 0) { - // this is the child, call childClient - childClient(); - - // break out so we don't fork bomb - break; - } else { - // this is the parent, replace the dead process with the new one - ::childForks[i] = newForkProcessID; - - qDebug("Replaced dead %d with new fork %d\n", processID, newForkProcessID); - - break; - } - } - } - } - -} - -void parentMonitor() { - - struct sigaction sa; - - memset(&sa, 0, sizeof(sa)); - sa.sa_handler = sigchldHandler; - - sigaction(SIGCHLD, &sa, NULL); - - pid_t childID = 0; - - // don't bail until all children have finished - while ((childID = waitpid(-1, NULL, 0))) { - if (errno == ECHILD) { - break; - } - } - - // delete the array of pid_t holding the forked process IDs - delete[] ::childForks; -} +#include "AssignmentClientMonitor.h" int main(int argc, char* argv[]) { - ::argc = argc; - ::argv = argv; setvbuf(stdout, NULL, _IOLBF, 0); // use the verbose message handler in Logging qInstallMessageHandler(Logging::verboseMessageHandler); - // start the Logging class with the parent's target name - Logging::setTargetName(PARENT_TARGET_NAME); - - const char CUSTOM_ASSIGNMENT_SERVER_HOSTNAME_OPTION[] = "-a"; - const char CUSTOM_ASSIGNMENT_SERVER_PORT_OPTION[] = "-p"; - - // grab the overriden assignment-server hostname from argv, if it exists - const char* customAssignmentServerHostname = getCmdOption(argc, (const char**)argv, CUSTOM_ASSIGNMENT_SERVER_HOSTNAME_OPTION); - const char* customAssignmentServerPortString = getCmdOption(argc,(const char**)argv, CUSTOM_ASSIGNMENT_SERVER_PORT_OPTION); - - if (customAssignmentServerHostname || customAssignmentServerPortString) { - - // set the custom port or default if it wasn't passed - unsigned short assignmentServerPort = customAssignmentServerPortString - ? atoi(customAssignmentServerPortString) : DEFAULT_DOMAIN_SERVER_PORT; - - // set the custom hostname or default if it wasn't passed - if (!customAssignmentServerHostname) { - customAssignmentServerHostname = DEFAULT_ASSIGNMENT_SERVER_HOSTNAME; - } - - ::customAssignmentSocket = HifiSockAddr(customAssignmentServerHostname, assignmentServerPort); - } - - const char ASSIGNMENT_TYPE_OVVERIDE_OPTION[] = "-t"; - const char* assignmentTypeString = getCmdOption(argc, (const char**)argv, ASSIGNMENT_TYPE_OVVERIDE_OPTION); - - if (assignmentTypeString) { - // the user is asking to only be assigned to a particular type of assignment - // so set that as the ::overridenAssignmentType to be used in requests - ::overiddenAssignmentType = (Assignment::Type) atoi(assignmentTypeString); - } - - const char ASSIGNMENT_POOL_OPTION[] = "--pool"; - ::assignmentPool = getCmdOption(argc, (const char**) argv, ASSIGNMENT_POOL_OPTION); - - const char* NUM_FORKS_PARAMETER = "-n"; const char* numForksString = getCmdOption(argc, (const char**)argv, NUM_FORKS_PARAMETER); - int processID = 0; + int numForks = 0; if (numForksString) { - ::numForks = atoi(numForksString); - qDebug("Starting %d assignment clients\n", ::numForks); - - ::childForks = new pid_t[::numForks]; - - // fire off as many children as we need (this is one less than the parent since the parent will run as well) - for (int i = 0; i < ::numForks; i++) { - processID = fork(); - - if (processID == 0) { - // this is in one of the children, break so we don't start a fork bomb - break; - } else { - // this is in the parent, save the ID of the forked process - childForks[i] = processID; - } - } + numForks = atoi(numForksString); } - if (processID == 0 || ::numForks == 0) { - return childClient(); + if (numForks) { + AssignmentClientMonitor monitor(argc, argv, numForks); + return monitor.exec(); } else { - parentMonitor(); + AssignmentClient client(argc, argv); + return client.exec(); } } \ No newline at end of file diff --git a/libraries/particles/src/Particle.cpp b/libraries/particles/src/Particle.cpp index 3323dec88b..5345e15bd0 100644 --- a/libraries/particles/src/Particle.cpp +++ b/libraries/particles/src/Particle.cpp @@ -58,6 +58,7 @@ void Particle::init(glm::vec3 position, float radius, rgbColor color, glm::vec3 _position = position; _radius = radius; + _mass = 1.0f; memcpy(_color, color, sizeof(_color)); _velocity = velocity; _damping = damping; @@ -67,6 +68,11 @@ void Particle::init(glm::vec3 position, float radius, rgbColor color, glm::vec3 _shouldDie = false; } +void Particle::setMass(float value) { + if (value > 0.0f) { + _mass = value; + } +} bool Particle::appendParticleData(OctreePacketData* packetData) const { diff --git a/libraries/particles/src/Particle.h b/libraries/particles/src/Particle.h index e93790eab7..fd950b7d0a 100644 --- a/libraries/particles/src/Particle.h +++ b/libraries/particles/src/Particle.h @@ -69,6 +69,7 @@ public: const rgbColor& getColor() const { return _color; } xColor getXColor() const { xColor color = { _color[RED_INDEX], _color[GREEN_INDEX], _color[BLUE_INDEX] }; return color; } float getRadius() const { return _radius; } + float getMass() const { return _mass; } const glm::vec3& getVelocity() const { return _velocity; } const glm::vec3& getGravity() const { return _gravity; } bool getInHand() const { return _inHand; } @@ -98,6 +99,7 @@ public: _color[BLUE_INDEX] = value.blue; } void setRadius(float value) { _radius = value; } + void setMass(float value); void setGravity(const glm::vec3& value) { _gravity = value; } void setInHand(bool inHand) { _inHand = inHand; } void setDamping(float value) { _damping = value; } @@ -149,6 +151,7 @@ protected: glm::vec3 _position; rgbColor _color; float _radius; + float _mass; glm::vec3 _velocity; uint32_t _id; static uint32_t _nextID; diff --git a/libraries/particles/src/ParticleCollisionSystem.cpp b/libraries/particles/src/ParticleCollisionSystem.cpp index 7538fc7f86..6711dc2d13 100644 --- a/libraries/particles/src/ParticleCollisionSystem.cpp +++ b/libraries/particles/src/ParticleCollisionSystem.cpp @@ -87,41 +87,50 @@ void ParticleCollisionSystem::updateCollisionWithVoxels(Particle* particle) { } } -void ParticleCollisionSystem::updateCollisionWithParticles(Particle* particle) { - glm::vec3 center = particle->getPosition() * (float)(TREE_SCALE); - float radius = particle->getRadius() * (float)(TREE_SCALE); - const float ELASTICITY = 1.4f; - const float DAMPING = 0.0f; +void ParticleCollisionSystem::updateCollisionWithParticles(Particle* particleA) { + glm::vec3 center = particleA->getPosition() * (float)(TREE_SCALE); + float radius = particleA->getRadius() * (float)(TREE_SCALE); + //const float ELASTICITY = 0.4f; + //const float DAMPING = 0.0f; const float COLLISION_FREQUENCY = 0.5f; glm::vec3 penetration; - Particle* penetratedParticle; - if (_particles->findSpherePenetration(center, radius, penetration, (void**)&penetratedParticle)) { + Particle* particleB; + if (_particles->findSpherePenetration(center, radius, penetration, (void**)&particleB)) { + // NOTE: 'penetration' is the depth that 'particleA' overlaps 'particleB'. + // That is, it points from A into B. + + // Even if the particles overlap... when the particles are already moving appart + // we don't want to count this as a collision. + glm::vec3 relativeVelocity = particleA->getVelocity() - particleB->getVelocity(); + if (glm::dot(relativeVelocity, penetration) > 0.0f) { + particleA->collisionWithParticle(particleB); + particleB->collisionWithParticle(particleA); - // let the particles run their collision scripts if they have them - particle->collisionWithParticle(penetratedParticle); - penetratedParticle->collisionWithParticle(particle); + glm::vec3 axis = glm::normalize(penetration); + glm::vec3 axialVelocity = glm::dot(relativeVelocity, axis) * axis; - penetration /= (float)(TREE_SCALE); - updateCollisionSound(particle, penetration, COLLISION_FREQUENCY); + // particles that are in hand are assigned an ureasonably large mass for collisions + // which effectively makes them immovable but allows the other ball to reflect correctly. + const float MAX_MASS = 1.0e6f; + float massA = (particleA->getInHand()) ? MAX_MASS : particleA->getMass(); + float massB = (particleB->getInHand()) ? MAX_MASS : particleB->getMass(); + float totalMass = massA + massB; + + particleA->setVelocity(particleA->getVelocity() - axialVelocity * (2.0f * massB / totalMass)); - // apply a hard collision to both particles of half the penetration each + ParticleEditHandle particleEditHandle(_packetSender, _particles, particleA->getID()); + particleEditHandle.updateParticle(particleA->getPosition(), particleA->getRadius(), particleA->getXColor(), particleA->getVelocity(), + particleA->getGravity(), particleA->getDamping(), particleA->getInHand(), particleA->getScript()); - float particleShare, penetratedParticleShare; - if (particle->getInHand() && penetratedParticle->getInHand()) { - particleShare = 0.5f; - penetratedParticleShare = -0.5f; - } else if (particle->getInHand()) { - particleShare = 0.f; - penetratedParticleShare = -1.f; - } else if (penetratedParticle->getInHand()) { - particleShare = -1.f; - penetratedParticleShare = 0.f; - } else { - particleShare = 0.5f; - penetratedParticleShare = -0.5f; + particleB->setVelocity(particleB->getVelocity() + axialVelocity * (2.0f * massA / totalMass)); + + ParticleEditHandle penetratedparticleEditHandle(_packetSender, _particles, particleB->getID()); + penetratedparticleEditHandle.updateParticle(particleB->getPosition(), particleB->getRadius(), particleB->getXColor(), particleB->getVelocity(), + particleB->getGravity(), particleB->getDamping(), particleB->getInHand(), particleB->getScript()); + + penetration /= (float)(TREE_SCALE); + updateCollisionSound(particleA, penetration, COLLISION_FREQUENCY); } - applyHardCollision(particle, penetration * particleShare, ELASTICITY, DAMPING); - applyHardCollision(penetratedParticle, penetration * penetratedParticleShare, ELASTICITY, DAMPING); } } diff --git a/libraries/particles/src/ParticleTree.h b/libraries/particles/src/ParticleTree.h index 3f8b7fda09..7c8d724a17 100644 --- a/libraries/particles/src/ParticleTree.h +++ b/libraries/particles/src/ParticleTree.h @@ -12,11 +12,6 @@ #include #include "ParticleTreeElement.h" -class ParticleTreeUpdateArgs { -public: - std::vector _movingParticles; -}; - class NewlyCreatedParticleHook { public: virtual void particleCreated(const Particle& newParticle, Node* senderNode) = 0; diff --git a/libraries/particles/src/ParticleTreeElement.h b/libraries/particles/src/ParticleTreeElement.h index 34d4054d98..971f5f5481 100644 --- a/libraries/particles/src/ParticleTreeElement.h +++ b/libraries/particles/src/ParticleTreeElement.h @@ -19,7 +19,11 @@ class ParticleTree; class ParticleTreeElement; -class ParticleTreeUpdateArgs; + +class ParticleTreeUpdateArgs { +public: + std::vector _movingParticles; +}; class ParticleTreeElement : public OctreeElement { friend class ParticleTree; // to allow createElement to new us... @@ -95,4 +99,4 @@ protected: std::vector _particles; }; -#endif /* defined(__hifi__ParticleTreeElement__) */ \ No newline at end of file +#endif /* defined(__hifi__ParticleTreeElement__) */ diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp index 58d6e0476e..7ffe4edfde 100644 --- a/libraries/shared/src/GeometryUtil.cpp +++ b/libraries/shared/src/GeometryUtil.cpp @@ -31,105 +31,107 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec } } -bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction, - float combinedRadius, glm::vec3& penetration) { - float vectorLength = glm::length(penetratorToPenetratee); +// Computes the penetration between a point and a sphere (centered at the origin) +// if point is inside sphere: returns true and stores the result in 'penetration' +// (the vector that would move the point outside the sphere) +// otherwise returns false +bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius, + glm::vec3& penetration) { + float vectorLength = glm::length(point); if (vectorLength < EPSILON) { - penetration = direction * combinedRadius; + penetration = defaultDirection * sphereRadius; return true; } - float distance = vectorLength - combinedRadius; + float distance = vectorLength - sphereRadius; if (distance < 0.0f) { - penetration = penetratorToPenetratee * (-distance / vectorLength); + penetration = point * (-distance / vectorLength); return true; } return false; } -bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeLocation, glm::vec3& penetration) { - return findSpherePenetration(penetrateeLocation - penetratorCenter, glm::vec3(0.0f, -1.0f, 0.0f), - penetratorRadius, penetration); +bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& point, glm::vec3& penetration) { + return findSpherePenetration(point - sphereCenter, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration); } -bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter, - float penetrateeRadius, glm::vec3& penetration) { - return findSpherePenetration(penetrateeCenter - penetratorLocation, glm::vec3(0.0f, -1.0f, 0.0f), - penetrateeRadius, penetration); +bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereCenter, + float sphereRadius, glm::vec3& penetration) { + return findSpherePenetration(sphereCenter - point, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration); } -bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) { - return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration); +bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius, + const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration) { + return findSpherePointPenetration(firstCenter, firstRadius + secondRadius, secondCenter, penetration); } -bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration) { - return findSpherePenetration(computeVectorFromPointToSegment(penetratorCenter, penetrateeStart, penetrateeEnd), - glm::vec3(0.0f, -1.0f, 0.0f), penetratorRadius, penetration); +bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration) { + return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd), + glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration); } -bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart, - const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration) { - return findSphereSegmentPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, - penetrateeStart, penetrateeEnd, penetration); +bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& capsuleStart, + const glm::vec3& capsuleEnd, float capsuleRadius, glm::vec3& penetration) { + return findSphereSegmentPenetration(sphereCenter, sphereRadius + capsuleRadius, + capsuleStart, capsuleEnd, penetration); } -bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart, - const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) { +bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart, + const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration) { // compute the projection of the point vector onto the segment vector - glm::vec3 segmentVector = penetrateeEnd - penetrateeStart; + glm::vec3 segmentVector = capsuleEnd - capsuleStart; float lengthSquared = glm::dot(segmentVector, segmentVector); if (lengthSquared < EPSILON) { // start and end the same - return findPointSpherePenetration(penetratorLocation, penetrateeStart, - glm::max(penetrateeStartRadius, penetrateeEndRadius), penetration); + return findPointSpherePenetration(point, capsuleStart, + glm::max(startRadius, endRadius), penetration); } - float proj = glm::dot(penetratorLocation - penetrateeStart, segmentVector) / lengthSquared; + float proj = glm::dot(point - capsuleStart, segmentVector) / lengthSquared; if (proj <= 0.0f) { // closest to the start - return findPointSpherePenetration(penetratorLocation, penetrateeStart, penetrateeStartRadius, penetration); + return findPointSpherePenetration(point, capsuleStart, startRadius, penetration); } else if (proj >= 1.0f) { // closest to the end - return findPointSpherePenetration(penetratorLocation, penetrateeEnd, penetrateeEndRadius, penetration); + return findPointSpherePenetration(point, capsuleEnd, endRadius, penetration); } else { // closest to the middle - return findPointSpherePenetration(penetratorLocation, penetrateeStart + segmentVector * proj, - glm::mix(penetrateeStartRadius, penetrateeEndRadius, proj), penetration); + return findPointSpherePenetration(point, capsuleStart + segmentVector * proj, + glm::mix(startRadius, endRadius, proj), penetration); } } -bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter, - float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, - float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) { - return findPointCapsuleConePenetration(penetratorCenter, penetrateeStart, penetrateeEnd, - penetrateeStartRadius + penetratorRadius, penetrateeEndRadius + penetratorRadius, penetration); +bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, + float sphereRadius, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, + float startRadius, float endRadius, glm::vec3& penetration) { + return findPointCapsuleConePenetration(sphereCenter, capsuleStart, capsuleEnd, + startRadius + sphereRadius, endRadius + sphereRadius, penetration); } -bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec4& penetrateePlane, glm::vec3& penetration) { - float distance = glm::dot(penetrateePlane, glm::vec4(penetratorCenter, 1.0f)) - penetratorRadius; +bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec4& plane, glm::vec3& penetration) { + float distance = glm::dot(plane, glm::vec4(sphereCenter, 1.0f)) - sphereRadius; if (distance < 0.0f) { - penetration = glm::vec3(penetrateePlane) * distance; + penetration = glm::vec3(plane) * distance; return true; } return false; } -bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) { - if (findSphereCapsulePenetration(penetrateeCenter, penetrateeRadius, - penetratorStart, penetratorEnd, penetratorRadius, penetration)) { +bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius, + const glm::vec3& sphereCenter, float sphereRadius, glm::vec3& penetration) { + if (findSphereCapsulePenetration(sphereCenter, sphereRadius, + capsuleStart, capsuleEnd, capsuleRadius, penetration)) { penetration = -penetration; return true; } return false; } -bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec4& penetrateePlane, glm::vec3& penetration) { - float distance = glm::min(glm::dot(penetrateePlane, glm::vec4(penetratorStart, 1.0f)), - glm::dot(penetrateePlane, glm::vec4(penetratorEnd, 1.0f))) - penetratorRadius; +bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius, + const glm::vec4& plane, glm::vec3& penetration) { + float distance = glm::min(glm::dot(plane, glm::vec4(capsuleStart, 1.0f)), + glm::dot(plane, glm::vec4(capsuleEnd, 1.0f))) - capsuleRadius; if (distance < 0.0f) { - penetration = glm::vec3(penetrateePlane) * distance; + penetration = glm::vec3(plane) * distance; return true; } return false; diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h index 8044039e55..2b782b43ae 100644 --- a/libraries/shared/src/GeometryUtil.h +++ b/libraries/shared/src/GeometryUtil.h @@ -13,39 +13,45 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end); -bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction, - float combinedRadius, glm::vec3& penetration); +/// Computes the penetration between a point and a sphere (centered at the origin) +/// \param point the point location relative to sphere center (origin) +/// \param defaultDirection the direction of the pentration when the point is near the origin +/// \param sphereRadius the radius of the sphere +/// \param penetration the displacement that would move the point out of penetration with the sphere +/// \return true if point is inside sphere, otherwise false +bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, + float sphereRadius, glm::vec3& penetration); -bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeLocation, glm::vec3& penetration); +bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& point, glm::vec3& penetration); -bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter, - float penetrateeRadius, glm::vec3& penetration); +bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereCenter, + float sphereRadius, glm::vec3& penetration); -bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration); +bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius, + const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration); -bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration); +bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration); -bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart, - const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration); +bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& capsuleStart, + const glm::vec3& capsuleEnd, float capsuleRadius, glm::vec3& penetration); -bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart, - const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration); +bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart, + const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration); -bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter, - float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, - float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration); +bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, + float startRadius, float endRadius, glm::vec3& penetration); -bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, - const glm::vec4& penetrateePlane, glm::vec3& penetration); +bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, + const glm::vec4& plane, glm::vec3& penetration); -bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration); +bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius, + const glm::vec3& sphereCenter, float sphereRadius, glm::vec3& penetration); -bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius, - const glm::vec4& penetrateePlane, glm::vec3& penetration); +bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius, + const glm::vec4& plane, glm::vec3& penetration); glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration);