Showing the changes to libraries/entities/src/EntityCollisionSystem.cpp (1 of 3 files). Show all
Unified · Split
| @@ -0,0 +1,336 @@ | |||
| 1 | // | ||
| 2 | // EntityCollisionSystem.cpp | ||
| 3 | // libraries/entities/src | ||
| 4 | // | ||
| 5 | // Created by Brad Hefta-Gaub on 9/23/14. | ||
| 6 | // Copyright 2013-2014 High Fidelity, Inc. | ||
| 7 | // | ||
| 8 | // Distributed under the Apache License, Version 2.0. | ||
| 9 | // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html | ||
| 10 | // | ||
| 11 | |||
| 12 | #include <algorithm> | ||
| 13 | #include <AbstractAudioInterface.h> | ||
| 14 | #include <VoxelTree.h> | ||
| 15 | #include <AvatarData.h> | ||
| 16 | #include <CollisionInfo.h> | ||
| 17 | #include <HeadData.h> | ||
| 18 | #include <HandData.h> | ||
| 19 | #include <PerfStat.h> | ||
| 20 | #include <SphereShape.h> | ||
| 21 | |||
| 22 | #include "EntityCollisionSystem.h" | ||
| 23 | #include "EntityEditPacketSender.h" | ||
| 24 | #include "EntityItem.h" | ||
| 25 | #include "EntityTreeElement.h" | ||
| 26 | #include "EntityTree.h" | ||
| 27 | |||
| 28 | const int MAX_COLLISIONS_PER_Entity = 16; | ||
| 29 | |||
| 30 | EntityCollisionSystem::EntityCollisionSystem() | ||
| 31 | : SimpleEntitySimulation(), | ||
| 32 | _packetSender(NULL), | ||
| 33 | _voxels(NULL), | ||
| 34 | _audio(NULL), | ||
| 35 | _avatars(NULL), | ||
| 36 | _collisions(MAX_COLLISIONS_PER_Entity) { | ||
| 37 | } | ||
| 38 | |||
| 39 | void EntityCollisionSystem::init(EntityEditPacketSender* packetSender, | ||
| 40 | EntityTree* entities, VoxelTree* voxels, AbstractAudioInterface* audio, | ||
| 41 | AvatarHashMap* avatars) { | ||
| 42 | assert(entities); | ||
| 43 | setEntityTree(entities); | ||
| 44 | _packetSender = packetSender; | ||
| 45 | _voxels = voxels; | ||
| 46 | _audio = audio; | ||
| 47 | _avatars = avatars; | ||
| 48 | } | ||
| 49 | |||
| 50 | EntityCollisionSystem::~EntityCollisionSystem() { | ||
| 51 | } | ||
| 52 | |||
| 53 | void EntityCollisionSystem::updateCollisions() { | ||
| 54 | PerformanceTimer perfTimer("collisions"); | ||
| 55 | assert(_entityTree); | ||
| 56 | // update all Entities | ||
| 57 | if (_entityTree->tryLockForRead()) { | ||
| 58 | foreach (EntityItem* entity, _movingEntities) { | ||
| 59 | checkEntity(entity); | ||
| 60 | } | ||
| 61 | _entityTree->unlock(); | ||
| 62 | } | ||
| 63 | } | ||
| 64 | |||
| 65 | |||
| 66 | void EntityCollisionSystem::checkEntity(EntityItem* entity) { | ||
| 67 | updateCollisionWithVoxels(entity); | ||
| 68 | updateCollisionWithEntities(entity); | ||
| 69 | updateCollisionWithAvatars(entity); | ||
| 70 | } | ||
| 71 | |||
| 72 | void EntityCollisionSystem::emitGlobalEntityCollisionWithVoxel(EntityItem* entity, | ||
| 73 | VoxelDetail* voxelDetails, const CollisionInfo& collision) { | ||
| 74 | EntityItemID entityItemID = entity->getEntityItemID(); | ||
| 75 | emit entityCollisionWithVoxel(entityItemID, *voxelDetails, collision); | ||
| 76 | } | ||
| 77 | |||
| 78 | void EntityCollisionSystem::emitGlobalEntityCollisionWithEntity(EntityItem* entityA, | ||
| 79 | EntityItem* entityB, const CollisionInfo& collision) { | ||
| 80 | |||
| 81 | EntityItemID idA = entityA->getEntityItemID(); | ||
| 82 | EntityItemID idB = entityB->getEntityItemID(); | ||
| 83 | emit entityCollisionWithEntity(idA, idB, collision); | ||
| 84 | } | ||
| 85 | |||
| 86 | void EntityCollisionSystem::updateCollisionWithVoxels(EntityItem* entity) { | ||
| 87 | |||
| 88 | if (entity->getIgnoreForCollisions() || !entity->getCollisionsWillMove()) { | ||
| 89 | return; // bail early if this entity is to be ignored or wont move | ||
| 90 | } | ||
| 91 | |||
| 92 | glm::vec3 center = entity->getPosition() * (float)(TREE_SCALE); | ||
| 93 | float radius = entity->getRadius() * (float)(TREE_SCALE); | ||
| 94 | const float ELASTICITY = 0.4f; | ||
| 95 | const float DAMPING = 0.05f; | ||
| 96 | CollisionInfo collisionInfo; | ||
| 97 | collisionInfo._damping = DAMPING; | ||
| 98 | collisionInfo._elasticity = ELASTICITY; | ||
| 99 | VoxelDetail* voxelDetails = NULL; | ||
| 100 | if (_voxels->findSpherePenetration(center, radius, collisionInfo._penetration, (void**)&voxelDetails)) { | ||
| 101 | |||
| 102 | // findSpherePenetration() only computes the penetration but we also want some other collision info | ||
| 103 | // so we compute it ourselves here. Note that we must multiply scale by TREE_SCALE when feeding | ||
| 104 | // the results to systems outside of this octree reference frame. | ||
| 105 | collisionInfo._contactPoint = (float)TREE_SCALE * (entity->getPosition() + entity->getRadius() * glm::normalize(collisionInfo._penetration)); | ||
| 106 | // let the global script run their collision scripts for Entities if they have them | ||
| 107 | emitGlobalEntityCollisionWithVoxel(entity, voxelDetails, collisionInfo); | ||
| 108 | |||
| 109 | // we must scale back down to the octree reference frame before updating the Entity properties | ||
| 110 | collisionInfo._penetration /= (float)(TREE_SCALE); | ||
| 111 | collisionInfo._contactPoint /= (float)(TREE_SCALE); | ||
| 112 | |||
| 113 | applyHardCollision(entity, collisionInfo); | ||
| 114 | delete voxelDetails; // cleanup returned details | ||
| 115 | } | ||
| 116 | } | ||
| 117 | |||
| 118 | void EntityCollisionSystem::updateCollisionWithEntities(EntityItem* entityA) { | ||
| 119 | |||
| 120 | if (entityA->getIgnoreForCollisions()) { | ||
| 121 | return; // bail early if this entity is to be ignored... | ||
| 122 | } | ||
| 123 | |||
| 124 | // don't collide entities with unknown IDs, | ||
| 125 | if (!entityA->isKnownID()) { | ||
| 126 | return; | ||
| 127 | } | ||
| 128 | |||
| 129 | glm::vec3 penetration; | ||
| 130 | EntityItem* entityB = NULL; | ||
| 131 | |||
| 132 | const int MAX_COLLISIONS_PER_ENTITY = 32; | ||
| 133 | CollisionList collisions(MAX_COLLISIONS_PER_ENTITY); | ||
| 134 | bool shapeCollisionsAccurate = false; | ||
| 135 | |||
| 136 | bool shapeCollisions = _entityTree->findShapeCollisions(&entityA->getCollisionShapeInMeters(), | ||
| 137 | collisions, Octree::NoLock, &shapeCollisionsAccurate); | ||
| 138 | |||
| 139 | if (shapeCollisions) { | ||
| 140 | for(int i = 0; i < collisions.size(); i++) { | ||
| 141 | |||
| 142 | CollisionInfo* collision = collisions[i]; | ||
| 143 | penetration = collision->_penetration; | ||
| 144 | entityB = static_cast<EntityItem*>(collision->_extraData); | ||
| 145 | |||
| 146 | // don't collide entities with unknown IDs, | ||
| 147 | if (!entityB->isKnownID()) { | ||
| 148 | continue; // skip this loop pass if the entity has an unknown ID | ||
| 149 | } | ||
| 150 | |||
| 151 | // NOTE: 'penetration' is the depth that 'entityA' overlaps 'entityB'. It points from A into B. | ||
| 152 | glm::vec3 penetrationInTreeUnits = penetration / (float)(TREE_SCALE); | ||
| 153 | |||
| 154 | // Even if the Entities overlap... when the Entities are already moving appart | ||
| 155 | // we don't want to count this as a collision. | ||
| 156 | glm::vec3 relativeVelocity = entityA->getVelocity() - entityB->getVelocity(); | ||
| 157 | |||
| 158 | bool fullyEnclosedCollision = glm::length(penetrationInTreeUnits) > entityA->getLargestDimension(); | ||
| 159 | |||
| 160 | bool wantToMoveA = entityA->getCollisionsWillMove(); | ||
| 161 | bool wantToMoveB = entityB->getCollisionsWillMove(); | ||
| 162 | bool movingTowardEachOther = glm::dot(relativeVelocity, penetrationInTreeUnits) > 0.0f; | ||
| 163 | |||
| 164 | // only do collisions if the entities are moving toward each other and one or the other | ||
| 165 | // of the entities are movable from collisions | ||
| 166 | bool doCollisions = !fullyEnclosedCollision && movingTowardEachOther && (wantToMoveA || wantToMoveB); | ||
| 167 | |||
| 168 | if (doCollisions) { | ||
| 169 | |||
| 170 | quint64 now = usecTimestampNow(); | ||
| 171 | |||
| 172 | CollisionInfo collision; | ||
| 173 | collision._penetration = penetration; | ||
| 174 | // for now the contactPoint is the average between the the two paricle centers | ||
| 175 | collision._contactPoint = (0.5f * (float)TREE_SCALE) * (entityA->getPosition() + entityB->getPosition()); | ||
| 176 | emitGlobalEntityCollisionWithEntity(entityA, entityB, collision); | ||
| 177 | |||
| 178 | glm::vec3 axis = glm::normalize(penetration); | ||
| 179 | glm::vec3 axialVelocity = glm::dot(relativeVelocity, axis) * axis; | ||
| 180 | |||
| 181 | float massA = entityA->getMass(); | ||
| 182 | float massB = entityB->getMass(); | ||
| 183 | float totalMass = massA + massB; | ||
| 184 | float massRatioA = (2.0f * massB / totalMass); | ||
| 185 | float massRatioB = (2.0f * massA / totalMass); | ||
| 186 | |||
| 187 | // in the event that one of our entities is non-moving, then fix up these ratios | ||
| 188 | if (wantToMoveA && !wantToMoveB) { | ||
| 189 | massRatioA = 2.0f; | ||
| 190 | massRatioB = 0.0f; | ||
| 191 | } | ||
| 192 | |||
| 193 | if (!wantToMoveA && wantToMoveB) { | ||
| 194 | massRatioA = 0.0f; | ||
| 195 | massRatioB = 2.0f; | ||
| 196 | } | ||
| 197 | |||
| 198 | // unless the entity is configured to not be moved by collision, calculate it's new position | ||
| 199 | // and velocity and apply it | ||
| 200 | if (wantToMoveA) { | ||
| 201 | // handle Entity A | ||
| 202 | glm::vec3 newVelocityA = entityA->getVelocity() - axialVelocity * massRatioA; | ||
| 203 | glm::vec3 newPositionA = entityA->getPosition() - 0.5f * penetrationInTreeUnits; | ||
| 204 | |||
| 205 | EntityItemProperties propertiesA = entityA->getProperties(); | ||
| 206 | EntityItemID idA(entityA->getID()); | ||
| 207 | propertiesA.setVelocity(newVelocityA * (float)TREE_SCALE); | ||
| 208 | propertiesA.setPosition(newPositionA * (float)TREE_SCALE); | ||
| 209 | propertiesA.setLastEdited(now); | ||
| 210 | |||
| 211 | _entityTree->updateEntity(idA, propertiesA); | ||
| 212 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idA, propertiesA); | ||
| 213 | } | ||
| 214 | |||
| 215 | // unless the entity is configured to not be moved by collision, calculate it's new position | ||
| 216 | // and velocity and apply it | ||
| 217 | if (wantToMoveB) { | ||
| 218 | glm::vec3 newVelocityB = entityB->getVelocity() + axialVelocity * massRatioB; | ||
| 219 | glm::vec3 newPositionB = entityB->getPosition() + 0.5f * penetrationInTreeUnits; | ||
| 220 | |||
| 221 | EntityItemProperties propertiesB = entityB->getProperties(); | ||
| 222 | |||
| 223 | EntityItemID idB(entityB->getID()); | ||
| 224 | propertiesB.setVelocity(newVelocityB * (float)TREE_SCALE); | ||
| 225 | propertiesB.setPosition(newPositionB * (float)TREE_SCALE); | ||
| 226 | propertiesB.setLastEdited(now); | ||
| 227 | |||
| 228 | _entityTree->updateEntity(idB, propertiesB); | ||
| 229 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idB, propertiesB); | ||
| 230 | } | ||
| 231 | } | ||
| 232 | } | ||
| 233 | } | ||
| 234 | } | ||
| 235 | |||
| 236 | void EntityCollisionSystem::updateCollisionWithAvatars(EntityItem* entity) { | ||
| 237 | |||
| 238 | // Entities that are in hand, don't collide with avatars | ||
| 239 | if (!_avatars) { | ||
| 240 | return; | ||
| 241 | } | ||
| 242 | |||
| 243 | if (entity->getIgnoreForCollisions() || !entity->getCollisionsWillMove()) { | ||
| 244 | return; // bail early if this entity is to be ignored or wont move | ||
| 245 | } | ||
| 246 | |||
| 247 | glm::vec3 center = entity->getPosition() * (float)(TREE_SCALE); | ||
| 248 | float radius = entity->getRadius() * (float)(TREE_SCALE); | ||
| 249 | const float ELASTICITY = 0.9f; | ||
| 250 | const float DAMPING = 0.1f; | ||
| 251 | glm::vec3 penetration; | ||
| 252 | |||
| 253 | _collisions.clear(); | ||
| 254 | foreach (const AvatarSharedPointer& avatarPointer, _avatars->getAvatarHash()) { | ||
| 255 | AvatarData* avatar = avatarPointer.data(); | ||
| 256 | |||
| 257 | float totalRadius = avatar->getBoundingRadius() + radius; | ||
| 258 | glm::vec3 relativePosition = center - avatar->getPosition(); | ||
| 259 | if (glm::dot(relativePosition, relativePosition) > (totalRadius * totalRadius)) { | ||
| 260 | continue; | ||
| 261 | } | ||
| 262 | |||
| 263 | if (avatar->findSphereCollisions(center, radius, _collisions)) { | ||
| 264 | int numCollisions = _collisions.size(); | ||
| 265 | for (int i = 0; i < numCollisions; ++i) { | ||
| 266 | CollisionInfo* collision = _collisions.getCollision(i); | ||
| 267 | collision->_damping = DAMPING; | ||
| 268 | collision->_elasticity = ELASTICITY; | ||
| 269 | |||
| 270 | collision->_addedVelocity /= (float)(TREE_SCALE); | ||
| 271 | glm::vec3 relativeVelocity = collision->_addedVelocity - entity->getVelocity(); | ||
| 272 | |||
| 273 | if (glm::dot(relativeVelocity, collision->_penetration) <= 0.0f) { | ||
| 274 | // only collide when Entity and collision point are moving toward each other | ||
| 275 | // (doing this prevents some "collision snagging" when Entity penetrates the object) | ||
| 276 | collision->_penetration /= (float)(TREE_SCALE); | ||
| 277 | applyHardCollision(entity, *collision); | ||
| 278 | } | ||
| 279 | } | ||
| 280 | } | ||
| 281 | } | ||
| 282 | } | ||
| 283 | |||
| 284 | void EntityCollisionSystem::applyHardCollision(EntityItem* entity, const CollisionInfo& collisionInfo) { | ||
| 285 | |||
| 286 | // don't collide entities with unknown IDs, | ||
| 287 | if (!entity->isKnownID()) { | ||
| 288 | return; | ||
| 289 | } | ||
| 290 | |||
| 291 | // HALTING_* params are determined using expected acceleration of gravity over some timescale. | ||
| 292 | // This is a HACK for entities that bounce in a 1.0 gravitational field and should eventually be made more universal. | ||
| 293 | const float HALTING_ENTITY_PERIOD = 0.0167f; // ~1/60th of a second | ||
| 294 | const float HALTING_ENTITY_SPEED = 9.8 * HALTING_ENTITY_PERIOD / (float)(TREE_SCALE); | ||
| 295 | |||
| 296 | // | ||
| 297 | // Update the entity in response to a hard collision. Position will be reset exactly | ||
| 298 | // to outside the colliding surface. Velocity will be modified according to elasticity. | ||
| 299 | // | ||
| 300 | // if elasticity = 0.0, collision is inelastic (vel normal to collision is lost) | ||
| 301 | // if elasticity = 1.0, collision is 100% elastic. | ||
| 302 | // | ||
| 303 | glm::vec3 position = entity->getPosition(); | ||
| 304 | glm::vec3 velocity = entity->getVelocity(); | ||
| 305 | |||
| 306 | const float EPSILON = 0.0f; | ||
| 307 | glm::vec3 relativeVelocity = collisionInfo._addedVelocity - velocity; | ||
| 308 | float velocityDotPenetration = glm::dot(relativeVelocity, collisionInfo._penetration); | ||
| 309 | if (velocityDotPenetration < EPSILON) { | ||
| 310 | // entity is moving into collision surface | ||
| 311 | // | ||
| 312 | // TODO: do something smarter here by comparing the mass of the entity vs that of the other thing | ||
| 313 | // (other's mass could be stored in the Collision Info). The smaller mass should surrender more | ||
| 314 | // position offset and should slave more to the other's velocity in the static-friction case. | ||
| 315 | position -= collisionInfo._penetration; | ||
| 316 | |||
| 317 | if (glm::length(relativeVelocity) < HALTING_ENTITY_SPEED) { | ||
| 318 | // static friction kicks in and entities moves with colliding object | ||
| 319 | velocity = collisionInfo._addedVelocity; | ||
| 320 | } else { | ||
| 321 | glm::vec3 direction = glm::normalize(collisionInfo._penetration); | ||
| 322 | velocity += glm::dot(relativeVelocity, direction) * (1.0f + collisionInfo._elasticity) * direction; // dynamic reflection | ||
| 323 | velocity += glm::clamp(collisionInfo._damping, 0.0f, 1.0f) * (relativeVelocity - glm::dot(relativeVelocity, direction) * direction); // dynamic friction | ||
| 324 | } | ||
| 325 | } | ||
| 326 | |||
| 327 | EntityItemProperties properties = entity->getProperties(); | ||
| 328 | EntityItemID entityItemID(entity->getID()); | ||
| 329 | |||
| 330 | properties.setPosition(position * (float)TREE_SCALE); | ||
| 331 | properties.setVelocity(velocity * (float)TREE_SCALE); | ||
| 332 | properties.setLastEdited(usecTimestampNow()); | ||
| 333 | |||
| 334 | _entityTree->updateEntity(entityItemID, properties); | ||
| 335 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, entityItemID, properties); | ||
| 336 | } | ||