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