History

moving EntityCollisionSystem back to entities lib

Andrew Meadows committed 11 years ago

Commit 08149491 parent 3781c233 files · +8 −8Browse files

Showing the changes to libraries/entities/src/EntityCollisionSystem.cpp (1 of 3 files). Show all

Unified · Split

libraries/entities/src/EntityCollisionSystem.cpp+336−0
@@ -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+}