Showing the changes to libraries/entities/src/EntityCollisionSystem.cpp (1 of 11 files). Show all
Unified · Split
| @@ -0,0 +1,351 @@ | |||
| 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 <HeadData.h> | ||
| 17 | #include <HandData.h> | ||
| 18 | |||
| 19 | #include "EntityItem.h" | ||
| 20 | #include "EntityCollisionSystem.h" | ||
| 21 | #include "EntityEditPacketSender.h" | ||
| 22 | #include "EntityTree.h" | ||
| 23 | #include "EntityTreeElement.h" | ||
| 24 | |||
| 25 | const int MAX_COLLISIONS_PER_Entity = 16; | ||
| 26 | |||
| 27 | EntityCollisionSystem::EntityCollisionSystem(EntityEditPacketSender* packetSender, | ||
| 28 | EntityTree* Entities, VoxelTree* voxels, AbstractAudioInterface* audio, | ||
| 29 | AvatarHashMap* avatars) : _collisions(MAX_COLLISIONS_PER_Entity) { | ||
| 30 | init(packetSender, Entities, voxels, audio, avatars); | ||
| 31 | } | ||
| 32 | |||
| 33 | void EntityCollisionSystem::init(EntityEditPacketSender* packetSender, | ||
| 34 | EntityTree* Entities, VoxelTree* voxels, AbstractAudioInterface* audio, | ||
| 35 | AvatarHashMap* avatars) { | ||
| 36 | _packetSender = packetSender; | ||
| 37 | _entities = Entities; | ||
| 38 | _voxels = voxels; | ||
| 39 | _audio = audio; | ||
| 40 | _avatars = avatars; | ||
| 41 | } | ||
| 42 | |||
| 43 | EntityCollisionSystem::~EntityCollisionSystem() { | ||
| 44 | } | ||
| 45 | |||
| 46 | bool EntityCollisionSystem::updateOperation(OctreeElement* element, void* extraData) { | ||
| 47 | EntityCollisionSystem* system = static_cast<EntityCollisionSystem*>(extraData); | ||
| 48 | EntityTreeElement* entityTreeElement = static_cast<EntityTreeElement*>(element); | ||
| 49 | |||
| 50 | // iterate the Entities... | ||
| 51 | QList<EntityItem*>& entities = entityTreeElement->getEntities(); | ||
| 52 | uint16_t numberOfEntities = entities.size(); | ||
| 53 | for (uint16_t i = 0; i < numberOfEntities; i++) { | ||
| 54 | EntityItem* entity = entities[i]; | ||
| 55 | system->checkEntity(entity); | ||
| 56 | } | ||
| 57 | |||
| 58 | return true; | ||
| 59 | } | ||
| 60 | |||
| 61 | |||
| 62 | void EntityCollisionSystem::update() { | ||
| 63 | // update all Entities | ||
| 64 | if (_entities->tryLockForRead()) { | ||
| 65 | _entities->recurseTreeWithOperation(updateOperation, this); | ||
| 66 | _entities->unlock(); | ||
| 67 | } | ||
| 68 | } | ||
| 69 | |||
| 70 | |||
| 71 | void EntityCollisionSystem::checkEntity(EntityItem* entity) { | ||
| 72 | updateCollisionWithVoxels(entity); | ||
| 73 | updateCollisionWithEntities(entity); | ||
| 74 | updateCollisionWithAvatars(entity); | ||
| 75 | } | ||
| 76 | |||
| 77 | void EntityCollisionSystem::emitGlobalEntityCollisionWithVoxel(EntityItem* entity, | ||
| 78 | VoxelDetail* voxelDetails, const CollisionInfo& collision) { | ||
| 79 | EntityItemID entityItemID = entity->getEntityItemID(); | ||
| 80 | emit EntityCollisionWithVoxel(entityItemID, *voxelDetails, collision); | ||
| 81 | } | ||
| 82 | |||
| 83 | void EntityCollisionSystem::emitGlobalEntityCollisionWithEntity(EntityItem* entityA, | ||
| 84 | EntityItem* entityB, const CollisionInfo& collision) { | ||
| 85 | |||
| 86 | EntityItemID idA = entityA->getEntityItemID(); | ||
| 87 | EntityItemID idB = entityB->getEntityItemID(); | ||
| 88 | emit EntityCollisionWithEntity(idA, idB, collision); | ||
| 89 | } | ||
| 90 | |||
| 91 | void EntityCollisionSystem::updateCollisionWithVoxels(EntityItem* entity) { | ||
| 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 | const float COLLISION_FREQUENCY = 0.5f; | ||
| 97 | CollisionInfo collisionInfo; | ||
| 98 | collisionInfo._damping = DAMPING; | ||
| 99 | collisionInfo._elasticity = ELASTICITY; | ||
| 100 | VoxelDetail* voxelDetails = NULL; | ||
| 101 | if (_voxels->findSpherePenetration(center, radius, collisionInfo._penetration, (void**)&voxelDetails)) { | ||
| 102 | |||
| 103 | // let the Entities run their collision scripts if they have them | ||
| 104 | //entity->collisionWithVoxel(voxelDetails, collisionInfo._penetration); | ||
| 105 | |||
| 106 | // findSpherePenetration() only computes the penetration but we also want some other collision info | ||
| 107 | // so we compute it ourselves here. Note that we must multiply scale by TREE_SCALE when feeding | ||
| 108 | // the results to systems outside of this octree reference frame. | ||
| 109 | updateCollisionSound(entity, collisionInfo._penetration, COLLISION_FREQUENCY); | ||
| 110 | collisionInfo._contactPoint = (float)TREE_SCALE * (entity->getPosition() + entity->getRadius() * glm::normalize(collisionInfo._penetration)); | ||
| 111 | // let the global script run their collision scripts for Entities if they have them | ||
| 112 | emitGlobalEntityCollisionWithVoxel(entity, voxelDetails, collisionInfo); | ||
| 113 | |||
| 114 | // we must scale back down to the octree reference frame before updating the Entity properties | ||
| 115 | collisionInfo._penetration /= (float)(TREE_SCALE); | ||
| 116 | collisionInfo._contactPoint /= (float)(TREE_SCALE); | ||
| 117 | entity->applyHardCollision(collisionInfo); | ||
| 118 | queueEntityPropertiesUpdate(entity); | ||
| 119 | |||
| 120 | delete voxelDetails; // cleanup returned details | ||
| 121 | } | ||
| 122 | } | ||
| 123 | |||
| 124 | void EntityCollisionSystem::updateCollisionWithEntities(EntityItem* entityA) { | ||
| 125 | glm::vec3 center = entityA->getPosition() * (float)(TREE_SCALE); | ||
| 126 | float radius = entityA->getRadius() * (float)(TREE_SCALE); | ||
| 127 | //const float ELASTICITY = 0.4f; | ||
| 128 | //const float DAMPING = 0.0f; | ||
| 129 | const float COLLISION_FREQUENCY = 0.5f; | ||
| 130 | glm::vec3 penetration; | ||
| 131 | EntityItem* entityB; | ||
| 132 | if (_entities->findSpherePenetration(center, radius, penetration, (void**)&entityB, Octree::NoLock)) { | ||
| 133 | // NOTE: 'penetration' is the depth that 'entityA' overlaps 'entityB'. It points from A into B. | ||
| 134 | |||
| 135 | qDebug() << "penetration:" << penetration << "in meters"; | ||
| 136 | |||
| 137 | glm::vec3 penetrationInTreeUnits = penetration / (float)(TREE_SCALE); | ||
| 138 | |||
| 139 | qDebug() << "penetrationInTreeUnits:" << penetrationInTreeUnits; | ||
| 140 | |||
| 141 | // Even if the Entities overlap... when the Entities are already moving appart | ||
| 142 | // we don't want to count this as a collision. | ||
| 143 | glm::vec3 relativeVelocity = entityA->getVelocity() - entityB->getVelocity(); | ||
| 144 | |||
| 145 | qDebug() << "A old velocity:" << entityA->getVelocity() * (float)TREE_SCALE << "in meters"; | ||
| 146 | qDebug() << "B old velocity:" << entityB->getVelocity() * (float)TREE_SCALE << "in meters"; | ||
| 147 | qDebug() << "relativeVelocity:" << relativeVelocity * (float)TREE_SCALE << "in meters"; | ||
| 148 | |||
| 149 | bool movingTowardEachOther = glm::dot(relativeVelocity, penetrationInTreeUnits) > 0.0f; | ||
| 150 | bool doCollisions = true; | ||
| 151 | |||
| 152 | if (doCollisions) { | ||
| 153 | //entityA->collisionWithEntity(entityB, penetration); | ||
| 154 | //entityB->collisionWithEntity(entityA, penetration * -1.0f); // the penetration is reversed | ||
| 155 | |||
| 156 | CollisionInfo collision; | ||
| 157 | collision._penetration = penetration; | ||
| 158 | // for now the contactPoint is the average between the the two paricle centers | ||
| 159 | collision._contactPoint = (0.5f * (float)TREE_SCALE) * (entityA->getPosition() + entityB->getPosition()); | ||
| 160 | //emitGlobalEntityCollisionWithEntity(entityA, entityB, collision); | ||
| 161 | |||
| 162 | glm::vec3 axis = glm::normalize(penetration); | ||
| 163 | glm::vec3 axialVelocity = glm::dot(relativeVelocity, axis) * axis; | ||
| 164 | |||
| 165 | float massA = entityA->getMass(); | ||
| 166 | float massB = entityB->getMass(); | ||
| 167 | float totalMass = massA + massB; | ||
| 168 | qDebug() << "massA:" << massA; | ||
| 169 | qDebug() << "massB:" << massB; | ||
| 170 | qDebug() << "totalMass:" << totalMass; | ||
| 171 | |||
| 172 | // handle Entity A | ||
| 173 | |||
| 174 | qDebug() << "OLD - VALUES - entityA ----"; | ||
| 175 | qDebug() << "entityA->getVelocity():" << entityA->getVelocity() << "no scaling"; | ||
| 176 | qDebug() << "entityA->getVelocity():" << entityA->getVelocity() * (float)TREE_SCALE << "in meters"; | ||
| 177 | qDebug() << "entityA->getPosition():" << entityA->getPosition() * (float)TREE_SCALE << "in meters"; | ||
| 178 | |||
| 179 | |||
| 180 | glm::vec3 newVelocity = entityA->getVelocity() - axialVelocity * (2.0f * massB / totalMass); | ||
| 181 | glm::vec3 newPosition = entityA->getPosition() - 0.5f * penetrationInTreeUnits; | ||
| 182 | |||
| 183 | qDebug() << "A new Velocity:" << newVelocity * (float)TREE_SCALE << "in meters"; | ||
| 184 | qDebug() << "A new Position:" << newPosition * (float)TREE_SCALE << "in meters"; | ||
| 185 | |||
| 186 | EntityItemProperties propertiesA = entityA->getProperties(); | ||
| 187 | |||
| 188 | qDebug() << "propertiesA = entityA->getProperties() --- BEFORE SETTING ----"; | ||
| 189 | qDebug() << " propertiesA.getPosition():" << propertiesA.getPosition() << "in meters"; | ||
| 190 | qDebug() << " propertiesA.getVelocity():" << propertiesA.getVelocity() << "in meters"; | ||
| 191 | qDebug() << " propertiesA.getMaximumAACubeInTreeUnits():" << propertiesA.getMaximumAACubeInTreeUnits(); | ||
| 192 | |||
| 193 | EntityItemID idA(entityA->getID()); | ||
| 194 | propertiesA.setVelocity(newVelocity * (float)TREE_SCALE); | ||
| 195 | propertiesA.setPosition(newPosition * (float)TREE_SCALE); | ||
| 196 | |||
| 197 | qDebug() << "updateEntity(idA, propertiesA)...."; | ||
| 198 | qDebug() << " idA:" << idA; | ||
| 199 | qDebug() << " propertiesA.getPosition():" << propertiesA.getPosition() << "in meters"; | ||
| 200 | qDebug() << " propertiesA.getVelocity():" << propertiesA.getVelocity() << "in meters"; | ||
| 201 | qDebug() << " propertiesA.getMaximumAACubeInTreeUnits():" << propertiesA.getMaximumAACubeInTreeUnits(); | ||
| 202 | |||
| 203 | _entities->updateEntity(idA, propertiesA); | ||
| 204 | |||
| 205 | qDebug() << "AFTER updateEntity()...."; | ||
| 206 | qDebug() << " entityA->getVelocity():" << entityA->getVelocity() << "no scaling"; | ||
| 207 | qDebug() << " entityA->getPosition():" << entityA->getPosition() * (float)TREE_SCALE << "in meters"; | ||
| 208 | |||
| 209 | |||
| 210 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idA, propertiesA); | ||
| 211 | //queueEntityMessage(PacketTypeEntityAddOrEdit, entityID, properties); | ||
| 212 | |||
| 213 | qDebug() << "OLD - VALUES - entityB ----"; | ||
| 214 | qDebug() << " entityB->getVelocity():" << entityB->getVelocity() << "no scaling"; | ||
| 215 | qDebug() << " entityB->getPosition():" << entityB->getPosition() * (float)TREE_SCALE << "in meters"; | ||
| 216 | |||
| 217 | glm::vec3 newVelocityB = entityB->getVelocity() + axialVelocity * (2.0f * massA / totalMass); | ||
| 218 | glm::vec3 newPositionB = entityB->getPosition() + 0.5f * penetrationInTreeUnits; | ||
| 219 | |||
| 220 | qDebug() << "B new Velocity:" << newVelocityB << "no scaling"; | ||
| 221 | qDebug() << "B new Position:" << newPositionB * (float)TREE_SCALE << "in meters"; | ||
| 222 | |||
| 223 | EntityItemProperties propertiesB = entityB->getProperties(); | ||
| 224 | |||
| 225 | qDebug() << "propertiesB = entityB->getProperties()"; | ||
| 226 | qDebug() << " propertiesB.getPosition():" << propertiesB.getPosition() << "not scaled, should be meters"; | ||
| 227 | qDebug() << " propertiesB.getVelocity():" << propertiesB.getVelocity() << "not scaled, should be ???"; | ||
| 228 | qDebug() << " propertiesB.getMaximumAACubeInTreeUnits():" << propertiesB.getMaximumAACubeInTreeUnits(); | ||
| 229 | |||
| 230 | EntityItemID idB(entityB->getID()); | ||
| 231 | propertiesB.setVelocity(newVelocityB * (float)TREE_SCALE); | ||
| 232 | propertiesB.setPosition(newPositionB * (float)TREE_SCALE); | ||
| 233 | |||
| 234 | qDebug() << "updateEntity(idB, propertiesB)...."; | ||
| 235 | qDebug() << " idB:" << idB; | ||
| 236 | qDebug() << " propertiesB.getPosition():" << propertiesB.getPosition(); | ||
| 237 | qDebug() << " propertiesB.getVelocity():" << propertiesB.getVelocity(); | ||
| 238 | qDebug() << " propertiesB.getMaximumAACubeInTreeUnits():" << propertiesB.getMaximumAACubeInTreeUnits(); | ||
| 239 | |||
| 240 | _entities->updateEntity(idB, propertiesB); | ||
| 241 | |||
| 242 | qDebug() << "AFTER updateEntity()...."; | ||
| 243 | qDebug() << " entityB->getVelocity():" << entityB->getVelocity() << "no scaling"; | ||
| 244 | qDebug() << " entityB->getPosition():" << entityB->getPosition() * (float)TREE_SCALE << "in meters"; | ||
| 245 | |||
| 246 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, idB, propertiesB); | ||
| 247 | //_packetSender->releaseQueuedMessages(); | ||
| 248 | |||
| 249 | //updateCollisionSound(entityA, penetration, COLLISION_FREQUENCY); | ||
| 250 | } | ||
| 251 | } | ||
| 252 | } | ||
| 253 | |||
| 254 | void EntityCollisionSystem::updateCollisionWithAvatars(EntityItem* entity) { | ||
| 255 | |||
| 256 | #if 0 | ||
| 257 | // Entities that are in hand, don't collide with avatars | ||
| 258 | if (!_avatars) { | ||
| 259 | return; | ||
| 260 | } | ||
| 261 | |||
| 262 | glm::vec3 center = entity->getPosition() * (float)(TREE_SCALE); | ||
| 263 | float radius = entity->getRadius() * (float)(TREE_SCALE); | ||
| 264 | const float ELASTICITY = 0.9f; | ||
| 265 | const float DAMPING = 0.1f; | ||
| 266 | const float COLLISION_FREQUENCY = 0.5f; | ||
| 267 | glm::vec3 penetration; | ||
| 268 | |||
| 269 | _collisions.clear(); | ||
| 270 | foreach (const AvatarSharedPointer& avatarPointer, _avatars->getAvatarHash()) { | ||
| 271 | AvatarData* avatar = avatarPointer.data(); | ||
| 272 | |||
| 273 | float totalRadius = avatar->getBoundingRadius() + radius; | ||
| 274 | glm::vec3 relativePosition = center - avatar->getPosition(); | ||
| 275 | if (glm::dot(relativePosition, relativePosition) > (totalRadius * totalRadius)) { | ||
| 276 | continue; | ||
| 277 | } | ||
| 278 | |||
| 279 | if (avatar->findSphereCollisions(center, radius, _collisions)) { | ||
| 280 | int numCollisions = _collisions.size(); | ||
| 281 | for (int i = 0; i < numCollisions; ++i) { | ||
| 282 | CollisionInfo* collision = _collisions.getCollision(i); | ||
| 283 | collision->_damping = DAMPING; | ||
| 284 | collision->_elasticity = ELASTICITY; | ||
| 285 | |||
| 286 | collision->_addedVelocity /= (float)(TREE_SCALE); | ||
| 287 | glm::vec3 relativeVelocity = collision->_addedVelocity - entity->getVelocity(); | ||
| 288 | |||
| 289 | if (glm::dot(relativeVelocity, collision->_penetration) <= 0.f) { | ||
| 290 | // only collide when Entity and collision point are moving toward each other | ||
| 291 | // (doing this prevents some "collision snagging" when Entity penetrates the object) | ||
| 292 | updateCollisionSound(entity, collision->_penetration, COLLISION_FREQUENCY); | ||
| 293 | collision->_penetration /= (float)(TREE_SCALE); | ||
| 294 | entity->applyHardCollision(*collision); | ||
| 295 | queueEntityPropertiesUpdate(entity); | ||
| 296 | } | ||
| 297 | } | ||
| 298 | } | ||
| 299 | } | ||
| 300 | |||
| 301 | #endif | ||
| 302 | } | ||
| 303 | |||
| 304 | void EntityCollisionSystem::queueEntityPropertiesUpdate(EntityItem* entity) { | ||
| 305 | // queue the result for sending to the Entity server | ||
| 306 | EntityItemProperties properties = entity->getProperties(); | ||
| 307 | EntityItemID entityItemID(entity->getID()); | ||
| 308 | |||
| 309 | properties.setPosition(entity->getPosition() * (float)TREE_SCALE); | ||
| 310 | properties.setVelocity(entity->getVelocity() * (float)TREE_SCALE); | ||
| 311 | _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, entityItemID, properties); | ||
| 312 | } | ||
| 313 | |||
| 314 | |||
| 315 | void EntityCollisionSystem::updateCollisionSound(EntityItem* Entity, const glm::vec3 &penetration, float frequency) { | ||
| 316 | |||
| 317 | // consider whether to have the collision make a sound | ||
| 318 | const float AUDIBLE_COLLISION_THRESHOLD = 0.3f; | ||
| 319 | const float COLLISION_LOUDNESS = 1.f; | ||
| 320 | const float DURATION_SCALING = 0.004f; | ||
| 321 | const float NOISE_SCALING = 0.1f; | ||
| 322 | glm::vec3 velocity = Entity->getVelocity() * (float)(TREE_SCALE); | ||
| 323 | |||
| 324 | /* | ||
| 325 | // how do we want to handle this?? | ||
| 326 | // | ||
| 327 | glm::vec3 gravity = Entity->getGravity() * (float)(TREE_SCALE); | ||
| 328 | |||
| 329 | if (glm::length(gravity) > EPSILON) { | ||
| 330 | // If gravity is on, remove the effect of gravity on velocity for this | ||
| 331 | // frame, so that we are not constantly colliding with the surface | ||
| 332 | velocity -= _scale * glm::length(gravity) * GRAVITY_EARTH * deltaTime * glm::normalize(gravity); | ||
| 333 | } | ||
| 334 | */ | ||
| 335 | float normalSpeed = glm::dot(velocity, glm::normalize(penetration)); | ||
| 336 | // NOTE: it is possible for normalSpeed to be NaN at this point | ||
| 337 | // (sometimes the average penetration of a bunch of voxels is a zero length vector which cannot be normalized) | ||
| 338 | // however the check below will fail (NaN comparisons always fail) and everything will be fine. | ||
| 339 | |||
| 340 | if (normalSpeed > AUDIBLE_COLLISION_THRESHOLD) { | ||
| 341 | // Volume is proportional to collision velocity | ||
| 342 | // Base frequency is modified upward by the angle of the collision | ||
| 343 | // Noise is a function of the angle of collision | ||
| 344 | // Duration of the sound is a function of both base frequency and velocity of impact | ||
| 345 | float tangentialSpeed = glm::length(velocity) - normalSpeed; | ||
| 346 | _audio->startCollisionSound( std::min(COLLISION_LOUDNESS * normalSpeed, 1.f), | ||
| 347 | frequency * (1.f + tangentialSpeed / normalSpeed), | ||
| 348 | std::min(tangentialSpeed / normalSpeed * NOISE_SCALING, 1.f), | ||
| 349 | 1.f - DURATION_SCALING * powf(frequency, 0.5f) / normalSpeed, false); | ||
| 350 | } | ||
| 351 | } | ||