dali-physics/third-party/chipmunk2d/src/cpSpaceComponent.c

   1 /* Copyright (c) 2007 Scott Lembcke
   2  *
   3  * Permission is hereby granted, free of charge, to any person obtaining a copy
   4  * of this software and associated documentation files (the "Software"), to deal
   5  * in the Software without restriction, including without limitation the rights
   6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   7  * copies of the Software, and to permit persons to whom the Software is
   8  * furnished to do so, subject to the following conditions:
   9  *
  10  * The above copyright notice and this permission notice shall be included in
  11  * all copies or substantial portions of the Software.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19  * SOFTWARE.
  20  */
  21
  22 #include <string.h>
  23
  24 #include "chipmunk/chipmunk_private.h"
  25
  26 //MARK: Sleeping Functions
  27
  28 void
  29 cpSpaceActivateBody(cpSpace *space, cpBody *body)
  30 {
  31         cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to activate a non-dynamic body.");
  32
  33         if(space->locked){
  34                 // cpSpaceActivateBody() is called again once the space is unlocked
  35                 if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
  36         } else {
  37                 cpAssertSoft(body->sleeping.root == NULL && body->sleeping.next == NULL, "Internal error: Activating body non-NULL node pointers.");
  38                 cpArrayPush(space->dynamicBodies, body);
  39
  40                 CP_BODY_FOREACH_SHAPE(body, shape){
  41                         cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
  42                         cpSpatialIndexInsert(space->dynamicShapes, shape, shape->hashid);
  43                 }
  44
  45                 CP_BODY_FOREACH_ARBITER(body, arb){
  46                         cpBody *bodyA = arb->body_a;
  47
  48                         // Arbiters are shared between two bodies that are always woken up together.
  49                         // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
  50                         // The edge case is when static bodies are involved as the static bodies never actually sleep.
  51                         // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
  52                         if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){
  53                                 int numContacts = arb->count;
  54                                 struct cpContact *contacts = arb->contacts;
  55
  56                                 // Restore contact values back to the space's contact buffer memory
  57                                 arb->contacts = cpContactBufferGetArray(space);
  58                                 memcpy(arb->contacts, contacts, numContacts*sizeof(struct cpContact));
  59                                 cpSpacePushContacts(space, numContacts);
  60
  61                                 // Reinsert the arbiter into the arbiter cache
  62                                 const cpShape *a = arb->a, *b = arb->b;
  63                                 const cpShape *shape_pair[] = {a, b};
  64                                 cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
  65                                 cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, NULL, arb);
  66
  67                                 // Update the arbiter's state
  68                                 arb->stamp = space->stamp;
  69                                 cpArrayPush(space->arbiters, arb);
  70
  71                                 cpfree(contacts);
  72                         }
  73                 }
  74
  75                 CP_BODY_FOREACH_CONSTRAINT(body, constraint){
  76                         cpBody *bodyA = constraint->a;
  77                         if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayPush(space->constraints, constraint);
  78                 }
  79         }
  80 }
  81
  82 static void
  83 cpSpaceDeactivateBody(cpSpace *space, cpBody *body)
  84 {
  85         cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Internal error: Attempting to deactivate a non-dynamic body.");
  86
  87         cpArrayDeleteObj(space->dynamicBodies, body);
  88
  89         CP_BODY_FOREACH_SHAPE(body, shape){
  90                 cpSpatialIndexRemove(space->dynamicShapes, shape, shape->hashid);
  91                 cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid);
  92         }
  93
  94         CP_BODY_FOREACH_ARBITER(body, arb){
  95                 cpBody *bodyA = arb->body_a;
  96                 if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC){
  97                         cpSpaceUncacheArbiter(space, arb);
  98
  99                         // Save contact values to a new block of memory so they won't time out
 100                         size_t bytes = arb->count*sizeof(struct cpContact);
 101                         struct cpContact *contacts = (struct cpContact *)cpcalloc(1, bytes);
 102                         memcpy(contacts, arb->contacts, bytes);
 103                         arb->contacts = contacts;
 104                 }
 105         }
 106
 107         CP_BODY_FOREACH_CONSTRAINT(body, constraint){
 108                 cpBody *bodyA = constraint->a;
 109                 if(body == bodyA || cpBodyGetType(bodyA) == CP_BODY_TYPE_STATIC) cpArrayDeleteObj(space->constraints, constraint);
 110         }
 111 }
 112
 113 static inline cpBody *
 114 ComponentRoot(cpBody *body)
 115 {
 116         return (body ? body->sleeping.root : NULL);
 117 }
 118
 119 void
 120 cpBodyActivate(cpBody *body)
 121 {
 122         if(body != NULL && cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){
 123                 body->sleeping.idleTime = 0.0f;
 124
 125                 cpBody *root = ComponentRoot(body);
 126                 if(root && cpBodyIsSleeping(root)){
 127                         // TODO should cpBodyIsSleeping(root) be an assertion?
 128                         cpAssertSoft(cpBodyGetType(root) == CP_BODY_TYPE_DYNAMIC, "Internal Error: Non-dynamic body component root detected.");
 129
 130                         cpSpace *space = root->space;
 131                         cpBody *body = root;
 132                         while(body){
 133                                 cpBody *next = body->sleeping.next;
 134
 135                                 body->sleeping.idleTime = 0.0f;
 136                                 body->sleeping.root = NULL;
 137                                 body->sleeping.next = NULL;
 138                                 cpSpaceActivateBody(space, body);
 139
 140                                 body = next;
 141                         }
 142
 143                         cpArrayDeleteObj(space->sleepingComponents, root);
 144                 }
 145
 146                 CP_BODY_FOREACH_ARBITER(body, arb){
 147                         // Reset the idle timer of things the body is touching as well.
 148                         // That way things don't get left hanging in the air.
 149                         cpBody *other = (arb->body_a == body ? arb->body_b : arb->body_a);
 150                         if(cpBodyGetType(other) != CP_BODY_TYPE_STATIC) other->sleeping.idleTime = 0.0f;
 151                 }
 152         }
 153 }
 154
 155 void
 156 cpBodyActivateStatic(cpBody *body, cpShape *filter)
 157 {
 158         cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_STATIC, "cpBodyActivateStatic() called on a non-static body.");
 159
 160         CP_BODY_FOREACH_ARBITER(body, arb){
 161                 if(!filter || filter == arb->a || filter == arb->b){
 162                         cpBodyActivate(arb->body_a == body ? arb->body_b : arb->body_a);
 163                 }
 164         }
 165
 166         // TODO: should also activate joints?
 167 }
 168
 169 static inline void
 170 cpBodyPushArbiter(cpBody *body, cpArbiter *arb)
 171 {
 172         cpAssertSoft(cpArbiterThreadForBody(arb, body)->next == NULL, "Internal Error: Dangling contact graph pointers detected. (A)");
 173         cpAssertSoft(cpArbiterThreadForBody(arb, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (B)");
 174
 175         cpArbiter *next = body->arbiterList;
 176         cpAssertSoft(next == NULL || cpArbiterThreadForBody(next, body)->prev == NULL, "Internal Error: Dangling contact graph pointers detected. (C)");
 177         cpArbiterThreadForBody(arb, body)->next = next;
 178
 179         if(next) cpArbiterThreadForBody(next, body)->prev = arb;
 180         body->arbiterList = arb;
 181 }
 182
 183 static inline void
 184 ComponentAdd(cpBody *root, cpBody *body){
 185         body->sleeping.root = root;
 186
 187         if(body != root){
 188                 body->sleeping.next = root->sleeping.next;
 189                 root->sleeping.next = body;
 190         }
 191 }
 192
 193 static inline void
 194 FloodFillComponent(cpBody *root, cpBody *body)
 195 {
 196         // Kinematic bodies cannot be put to sleep and prevent bodies they are touching from sleeping.
 197         // Static bodies are effectively sleeping all the time.
 198         if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC){
 199                 cpBody *other_root = ComponentRoot(body);
 200                 if(other_root == NULL){
 201                         ComponentAdd(root, body);
 202                         CP_BODY_FOREACH_ARBITER(body, arb) FloodFillComponent(root, (body == arb->body_a ? arb->body_b : arb->body_a));
 203                         CP_BODY_FOREACH_CONSTRAINT(body, constraint) FloodFillComponent(root, (body == constraint->a ? constraint->b : constraint->a));
 204                 } else {
 205                         cpAssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph.");
 206                 }
 207         }
 208 }
 209
 210 static inline cpBool
 211 ComponentActive(cpBody *root, cpFloat threshold)
 212 {
 213         CP_BODY_FOREACH_COMPONENT(root, body){
 214                 if(body->sleeping.idleTime < threshold) return cpTrue;
 215         }
 216
 217         return cpFalse;
 218 }
 219
 220 void
 221 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
 222 {
 223         cpBool sleep = (space->sleepTimeThreshold != INFINITY);
 224         cpArray *bodies = space->dynamicBodies;
 225
 226 #ifndef NDEBUG
 227         for(int i=0; i<bodies->num; i++){
 228                 cpBody *body = (cpBody*)bodies->arr[i];
 229
 230                 cpAssertSoft(body->sleeping.next == NULL, "Internal Error: Dangling next pointer detected in contact graph.");
 231                 cpAssertSoft(body->sleeping.root == NULL, "Internal Error: Dangling root pointer detected in contact graph.");
 232         }
 233 #endif
 234
 235         // Calculate the kinetic energy of all the bodies.
 236         if(sleep){
 237                 cpFloat dv = space->idleSpeedThreshold;
 238                 cpFloat dvsq = (dv ? dv*dv : cpvlengthsq(space->gravity)*dt*dt);
 239
 240                 // update idling and reset component nodes
 241                 for(int i=0; i<bodies->num; i++){
 242                         cpBody *body = (cpBody*)bodies->arr[i];
 243
 244                         // TODO should make a separate array for kinematic bodies.
 245                         if(cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) continue;
 246
 247                         // Need to deal with infinite mass objects
 248                         cpFloat keThreshold = (dvsq ? body->m*dvsq : 0.0f);
 249                         body->sleeping.idleTime = (cpBodyKineticEnergy(body) > keThreshold ? 0.0f : body->sleeping.idleTime + dt);
 250                 }
 251         }
 252
 253         // Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
 254         cpArray *arbiters = space->arbiters;
 255         for(int i=0, count=arbiters->num; i<count; i++){
 256                 cpArbiter *arb = (cpArbiter*)arbiters->arr[i];
 257                 cpBody *a = arb->body_a, *b = arb->body_b;
 258
 259                 if(sleep){
 260                         // TODO checking cpBodyIsSleepin() redundant?
 261                         if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(a)) cpBodyActivate(a);
 262                         if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC || cpBodyIsSleeping(b)) cpBodyActivate(b);
 263                 }
 264
 265                 cpBodyPushArbiter(a, arb);
 266                 cpBodyPushArbiter(b, arb);
 267         }
 268
 269         if(sleep){
 270                 // Bodies should be held active if connected by a joint to a kinematic.
 271                 cpArray *constraints = space->constraints;
 272                 for(int i=0; i<constraints->num; i++){
 273                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
 274                         cpBody *a = constraint->a, *b = constraint->b;
 275
 276                         if(cpBodyGetType(b) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(a);
 277                         if(cpBodyGetType(a) == CP_BODY_TYPE_KINEMATIC) cpBodyActivate(b);
 278                 }
 279
 280                 // Generate components and deactivate sleeping ones
 281                 for(int i=0; i<bodies->num;){
 282                         cpBody *body = (cpBody*)bodies->arr[i];
 283
 284                         if(ComponentRoot(body) == NULL){
 285                                 // Body not in a component yet. Perform a DFS to flood fill mark
 286                                 // the component in the contact graph using this body as the root.
 287                                 FloodFillComponent(body, body);
 288
 289                                 // Check if the component should be put to sleep.
 290                                 if(!ComponentActive(body, space->sleepTimeThreshold)){
 291                                         cpArrayPush(space->sleepingComponents, body);
 292                                         CP_BODY_FOREACH_COMPONENT(body, other) cpSpaceDeactivateBody(space, other);
 293
 294                                         // cpSpaceDeactivateBody() removed the current body from the list.
 295                                         // Skip incrementing the index counter.
 296                                         continue;
 297                                 }
 298                         }
 299
 300                         i++;
 301
 302                         // Only sleeping bodies retain their component node pointers.
 303                         body->sleeping.root = NULL;
 304                         body->sleeping.next = NULL;
 305                 }
 306         }
 307 }
 308
 309 void
 310 cpBodySleep(cpBody *body)
 311 {
 312         cpBodySleepWithGroup(body, NULL);
 313 }
 314
 315 void
 316 cpBodySleepWithGroup(cpBody *body, cpBody *group){
 317         cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "Non-dynamic bodies cannot be put to sleep.");
 318
 319         cpSpace *space = body->space;
 320         cpAssertHard(!cpSpaceIsLocked(space), "Bodies cannot be put to sleep during a query or a call to cpSpaceStep(). Put these calls into a post-step callback.");
 321         cpAssertHard(cpSpaceGetSleepTimeThreshold(space) < INFINITY, "Sleeping is not enabled on the space. You cannot sleep a body without setting a sleep time threshold on the space.");
 322         cpAssertHard(group == NULL || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
 323
 324         if(cpBodyIsSleeping(body)){
 325                 cpAssertHard(ComponentRoot(body) == ComponentRoot(group), "The body is already sleeping and it's group cannot be reassigned.");
 326                 return;
 327         }
 328
 329         CP_BODY_FOREACH_SHAPE(body, shape) cpShapeCacheBB(shape);
 330         cpSpaceDeactivateBody(space, body);
 331
 332         if(group){
 333                 cpBody *root = ComponentRoot(group);
 334
 335                 body->sleeping.root = root;
 336                 body->sleeping.next = root->sleeping.next;
 337                 body->sleeping.idleTime = 0.0f;
 338
 339                 root->sleeping.next = body;
 340         } else {
 341                 body->sleeping.root = body;
 342                 body->sleeping.next = NULL;
 343                 body->sleeping.idleTime = 0.0f;
 344
 345                 cpArrayPush(space->sleepingComponents, body);
 346         }
 347
 348         cpArrayDeleteObj(space->dynamicBodies, body);
 349 }