src/jelly/Body.cpp

   1 /*
   2  * Copyright (c) 2007 Walaber
   3  * Modified by Ketmar // Invisible Vector
   4  *
   5  * Permission is hereby granted, free of charge, to any person obtaining a copy
   6  * of this software and associated documentation files (the "Software"), to deal
   7  * in the Software without restriction, including without limitation the rights
   8  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9  * copies of the Software, and to permit persons to whom the Software is
  10  * furnished to do so, subject to the following conditions:
  11  *
  12  * The above copyright notice and this permission notice shall be included in
  13  * all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  21  * THE SOFTWARE.
  22  */
  23 #include <stdio.h>
  24
  25 #include "Body.h"
  26
  27 #include "VectorTools.h"
  28 #include "World.h"
  29
  30
  31 namespace JellyPhysics {
  32
  33 void Body::BodyBoundary::log () const {
  34   printf("%s(%p)[%4.2f] |", (type == Begin ? "B" : (type == End ? "E" : "V")), body, value);
  35 }
  36
  37
  38 Body::Body (World *w) {
  39   mWorld = w;
  40   mScale = Vector2::One;
  41   mIsStatic = false;
  42   mKinematic = false;
  43
  44   mVelDamping = 0.999f;
  45   mObjectTag = NULL;
  46
  47   mMaterial = 0;
  48
  49   mPointCount = 0;
  50   mInvPC = 0.0f;
  51
  52   mBoundStart.body = this;
  53   mBoundEnd.body = this;
  54   mBoundEnd.type = BodyBoundary::End;
  55
  56   mIgnoreMe = false;
  57
  58   w->addBody(this);
  59 }
  60
  61
  62 Body::Body (World *w, const ClosedShape &shape, float massPerPoint, const Vector2 &position, float angleInRadians, const Vector2 &scale, bool kinematic) {
  63   mWorld = w;
  64   mDerivedPos = position;
  65   mDerivedAngle = angleInRadians;
  66   mLastAngle = mDerivedAngle;
  67   mScale = scale;
  68   mMaterial = 0;
  69   mPointCount = 0;
  70   mInvPC = 0.0f;
  71   mIsStatic = (massPerPoint == 0.0f);
  72   mKinematic = kinematic;
  73
  74   mVelDamping = 0.999f;
  75   mObjectTag = NULL;
  76
  77   mBoundStart.body = this;
  78   mBoundEnd.body = this;
  79   mBoundEnd.type = BodyBoundary::End;
  80
  81   mIgnoreMe = false;
  82
  83   setShape(shape);
  84   for (int i = 0; i < mPointCount; ++i) mPointMasses[i].setMass(massPerPoint);
  85
  86   updateAABB(0.0f, true);
  87   updateEdgeInfo(true);
  88   updateBoundaryValues(true);
  89
  90   w->addBody(this);
  91 }
  92
  93
  94 Body::Body (World *w, const ClosedShape &shape, std::vector<float> pointMasses, const Vector2 &position, float angleInRadians, const Vector2 &scale, bool kinematic) {
  95   mWorld = w;
  96   mDerivedPos = position;
  97   mDerivedAngle = angleInRadians;
  98   mLastAngle = mDerivedAngle;
  99   mScale = scale;
 100   mMaterial = 0;
 101   mPointCount = 0;
 102   mInvPC = 0.0f;
 103
 104   mIsStatic = false;
 105   mKinematic = kinematic;
 106
 107   mVelDamping = 0.999f;
 108   mObjectTag = NULL;
 109
 110   mBoundStart.body = this;
 111   mBoundEnd.body = this;
 112   mBoundEnd.type = BodyBoundary::End;
 113
 114   mIgnoreMe = false;
 115
 116   setShape(shape);
 117   for (int i = 0; i < mPointCount; ++i) mPointMasses[i].setMass(pointMasses[i]);
 118
 119   updateAABB(0.0f, true);
 120   updateEdgeInfo(true);
 121   updateBoundaryValues(true);
 122
 123   w->addBody(this);
 124 }
 125
 126
 127 Body::~Body () {
 128   mWorld->removeBody(this);
 129 }
 130
 131
 132 void Body::setShape (const ClosedShape &shape) {
 133   mBaseShape = shape;
 134   if (mBaseShape.getVertices().size() != (unsigned int)mPointCount) {
 135     mPointMasses.clear();
 136     mGlobalShape.clear();
 137     mEdgeInfo.clear();
 138     for (unsigned int i = mBaseShape.getVertices().size(); i > 0; --i) mGlobalShape.push_back(Vector2::Zero);
 139     mBaseShape.transformVertices(mDerivedPos, mDerivedAngle, mScale, mGlobalShape);
 140     for (unsigned int i = 0; i < mBaseShape.getVertices().size(); ++i) mPointMasses.push_back(PointMass(0.0f, mGlobalShape[i]));
 141
 142     EdgeInfo e;
 143     e.dir = Vector2::Zero;
 144     e.length = 0.0f;
 145
 146     for (unsigned int i = mBaseShape.getVertices().size(); i > 0; --i) mEdgeInfo.push_back(e);
 147
 148     mPointCount = mPointMasses.size();
 149     mInvPC = 1.0f/mPointCount;
 150   }
 151
 152   updateAABB(0.0f, true);
 153   updateEdgeInfo(true);
 154   updateBoundaryValues(true);
 155 }
 156
 157
 158 void Body::setMassAll (float mass) {
 159   if (isinf(mass)) mass = 0.0f;
 160   for (int i = 0; i < mPointCount; ++i) mPointMasses[i].setMass(mass);
 161   if (mass == 0.0f) mIsStatic = true;
 162 }
 163
 164
 165 void Body::setMassIndividual (int index, float mass) {
 166   if (index >= 0 && index < mPointCount) mPointMasses[index].setMass(mass);
 167 }
 168
 169
 170 void Body::setMassFromList (const std::vector<float> &masses) {
 171   if (masses.size() == (unsigned int)mPointCount) {
 172     for (int i = 0; i < mPointCount; ++i) mPointMasses[i].setMass(masses[i]);
 173   }
 174 }
 175
 176
 177 void Body::setPositionAngle (const Vector2 &pos, float angleInRadians, const Vector2 &scale) {
 178   mBaseShape.transformVertices(pos, angleInRadians, scale, mGlobalShape);
 179   for (int i = 0; i < mPointCount; ++i) mPointMasses[i].Position = mGlobalShape[i];
 180   mDerivedPos = pos;
 181   mDerivedAngle = angleInRadians;
 182 }
 183
 184
 185 void Body::derivePositionAndAngle (float elapsed) {
 186   // no need it this is a static body, or kinematically controlled
 187   if (mIsStatic || mKinematic) return;
 188   // if we are being ignored, be ignored!
 189   if (mIgnoreMe) return;
 190   // find the geometric center
 191   Vector2 center = Vector2::Zero;
 192   Vector2 vel = Vector2::Zero;
 193   for (PointMassList::const_iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it) {
 194     center += (*it).Position;
 195     vel += (*it).Velocity;
 196   }
 197   center *= mInvPC;
 198   vel *= mInvPC;
 199   mDerivedPos = center;
 200   mDerivedVel = vel;
 201   // find the average angle of all of the masses
 202   float angle = 0;
 203   int originalSign = 1;
 204   float originalAngle = 0;
 205   for (int i = 0; i < mPointCount; ++i) {
 206     Vector2 baseNorm = mBaseShape.getVertices()[i];
 207     baseNorm.normalise();
 208     Vector2 curNorm = mPointMasses[i].Position-mDerivedPos;
 209     curNorm.normalise();
 210     float dot = baseNorm.dotProduct(curNorm);
 211     if (dot > 1.0f) dot = 1.0f;
 212     if (dot < -1.0f) dot = -1.0f;
 213     float thisAngle = acosf(dot);
 214     if (!isCCW(baseNorm, curNorm)) thisAngle = -thisAngle;
 215     if (i == 0) {
 216       originalSign = (thisAngle >= 0.0f ? 1 : -1);
 217       originalAngle = thisAngle;
 218     } else {
 219       float diff = thisAngle-originalAngle;
 220       int thisSign = (thisAngle >= 0.0f ? 1 : -1);
 221       if (fabsf(diff) > PI && thisSign != originalSign) {
 222         thisAngle = (thisSign == -1 ? PI+(PI+thisAngle) : (PI-thisAngle)-PI);
 223       }
 224     }
 225     angle += thisAngle;
 226   }
 227   angle *= mInvPC;
 228   mDerivedAngle = angle;
 229   // now calculate the derived Omega, based on change in angle over time
 230   float angleChange = mDerivedAngle-mLastAngle;
 231   if (fabsf(angleChange) >= PI) {
 232     if (angleChange < 0.0f) angleChange += TWO_PI; else angleChange -= TWO_PI;
 233   }
 234   mDerivedOmega = angleChange/elapsed;
 235   mLastAngle = mDerivedAngle;
 236 }
 237
 238
 239 void Body::integrate (float elapsed) {
 240   if (mIsStatic || mIgnoreMe) return;
 241   for (PointMassList::iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it) (*it).integrateForce(elapsed);
 242 }
 243
 244
 245 void Body::dampenVelocity () {
 246   if (mIsStatic || mIgnoreMe) return;
 247   for (PointMassList::iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it) (*it).Velocity *= mVelDamping;
 248 }
 249
 250
 251 void Body::updateAABB (float elapsed, bool forceUpdate) {
 252   if ((!mIsStatic && !mIgnoreMe) || forceUpdate) {
 253     mAABB.clear();
 254     for (PointMassList::const_iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it) {
 255       Vector2 p = (*it).Position;
 256       mAABB.expandToInclude(p);
 257       // expanding for velocity only makes sense for dynamic objects.
 258       if (!mIsStatic) {
 259         p += (*it).Velocity*elapsed;
 260         mAABB.expandToInclude(p);
 261       }
 262     }
 263     //printf("Body: %d AABB: min[%f][%f] max[%f][%f]\n", this, mAABB.Min.X, mAABB.Min.Y, mAABB.Max.X, mAABB.Max.Y);
 264   }
 265 }
 266
 267
 268 void Body::updateBoundaryValues (bool forceUpdate) {
 269   if ((!mIsStatic && !mIgnoreMe) || forceUpdate) {
 270     mBoundStart.value = mAABB.Min.X;
 271     mBoundEnd.value = mAABB.Max.X;
 272   }
 273 }
 274
 275
 276 void Body::updateEdgeInfo (bool forceUpdate) {
 277   if ((!mIsStatic && !mIgnoreMe) || forceUpdate) {
 278     int c = mPointCount;
 279     //k8: trick
 280     int i = c-1;
 281     for (int n = 0; n < c; i = n++) {
 282       Vector2 e = mPointMasses[n].Position-mPointMasses[i].Position;
 283       mEdgeInfo[i].length = e.normalise();
 284       mEdgeInfo[i].dir = e;
 285       mEdgeInfo[i].slope = (fabsf(e.Y) < 1.0e-08 ? 0.0f : e.X/e.Y);
 286     }
 287   }
 288 }
 289
 290
 291 bool Body::contains (const Vector2 &pt) const {
 292   // basic idea: draw a line from the point to a point known to be outside the body.
 293   // count the number of lines in the polygon it intersects.
 294   // if that number is odd, we are inside. if it's even, we are outside.
 295   // in this implementation we will always use a line that moves off in the positive X direction from the point to simplify things.
 296   Vector2 endPt = Vector2(mAABB.Max.X+0.1f, pt.Y);
 297   // line we are testing against goes from pt -> endPt
 298   bool inside = false;
 299   Vector2 edgeSt = mPointMasses[0].Position;
 300   Vector2 edgeEnd;
 301   int c = mPointCount;
 302   //k8: trick
 303   int i = c-1;
 304   for (int n = 0; n < c; i = n++) {
 305     // the current edge is defined as the line from edgeSt -> edgeEnd
 306     edgeEnd = mPointMasses[n].Position;
 307     // perform check now
 308     if ((edgeSt.Y <= pt.Y && edgeEnd.Y > pt.Y) || (edgeSt.Y > pt.Y && edgeEnd.Y <= pt.Y)) {
 309       // this line crosses the test line at some point; does it do so within our test range?
 310       float slope = mEdgeInfo[i].slope; //(edgeEnd.X-edgeSt.X)/(edgeEnd.Y-edgeSt.Y);
 311       float hitX = edgeSt.X+((pt.Y-edgeSt.Y)*slope);
 312       if (hitX >= pt.X && hitX <= endPt.X) inside = !inside;
 313     }
 314     edgeSt = edgeEnd;
 315   }
 316   return inside;
 317 }
 318
 319
 320 float Body::getClosestPoint (const Vector2 &pt, Vector2 *hitPt, Vector2 *norm, int *pointA, int *pointB, float *edgeD) const {
 321   if (hitPt) *hitPt = Vector2::Zero;
 322   if (pointA) *pointA = -1;
 323   if (pointB) *pointB = -1;
 324   if (edgeD) *edgeD = 0.0f;
 325   if (norm) *norm = Vector2::Zero;
 326   float closestD = 1000.0f;
 327   int c = mPointCount;
 328   //k8: trick
 329   int i = c-1;
 330   for (int n = 0; n < c; i = n++) {
 331     Vector2 tempHit, tempNorm;
 332     float tempEdgeD;
 333     float dist = getClosestPointOnEdge(pt, i, &tempHit, &tempNorm, &tempEdgeD);
 334     if (dist < closestD) {
 335       closestD = dist;
 336       if (pointA) *pointA = i;
 337       if (pointB) *pointB = n;
 338       if (edgeD) *edgeD = tempEdgeD;
 339       if (norm) *norm = tempNorm;
 340       if (hitPt) *hitPt = tempHit;
 341     }
 342   }
 343   return closestD;
 344 }
 345
 346
 347 float Body::getClosestPointOnEdge (const Vector2 &pt, int edgeNum, Vector2 *hitPt, Vector2 *norm, float *edgeD) const {
 348   float dist = 0.0f;
 349   Vector2 ptA = mPointMasses[edgeNum].Position;
 350   Vector2 ptB = mPointMasses[edgeNum < mPointCount-1 ? edgeNum+1 : 0].Position;
 351   Vector2 toP = pt-ptA;
 352   Vector2 E = mEdgeInfo[edgeNum].dir;
 353   // get the length of the edge, and use that to normalize the vector
 354   float edgeLength = mEdgeInfo[edgeNum].length;
 355   // normal
 356   Vector2 n = E.getPerpendicular();
 357   if (hitPt) *hitPt = Vector2::Zero;
 358   if (norm) *norm = Vector2::Zero;
 359   if (edgeD) *edgeD = 0.0f;
 360   // calculate the distance!
 361   float x = toP.dotProduct(E);
 362   if (x <= 0.0f) {
 363     // x is outside the line segment, distance is from pt to ptA
 364     dist = (pt-ptA).length();
 365     if (hitPt) *hitPt = ptA;
 366     if (edgeD) *edgeD = 0.0f;
 367     if (norm) *norm = n;
 368   } else if (x >= edgeLength) {
 369     // x is outside of the line segment, distance is from pt to ptB
 370     dist = (pt-ptB).length();
 371     if (hitPt) *hitPt = ptB;
 372     if (edgeD) *edgeD = 1.0f;
 373     if (norm) *norm = n;
 374   } else {
 375     // point lies somewhere on the line segment
 376     dist = fabsf(toP.crossProduct(E));
 377     if (hitPt) *hitPt = ptA+(E*x);
 378     if (edgeD) *edgeD = x/edgeLength;
 379     if (norm) *norm = n;
 380   }
 381   return dist;
 382 }
 383
 384
 385 float Body::getClosestPointOnEdgeSquared (const Vector2 &pt, int edgeNum, Vector2 *hitPt, Vector2 *norm, float *edgeD) const {
 386   float dist = 0.0f;
 387   Vector2 ptA = mPointMasses[edgeNum].Position;
 388   Vector2 ptB = mPointMasses[edgeNum < mPointCount-1 ? edgeNum+1 : 0].Position;
 389   Vector2 toP = pt-ptA;
 390   Vector2 E = mEdgeInfo[edgeNum].dir;
 391   // get the length of the edge, and use that to normalize the vector
 392   float edgeLength = mEdgeInfo[edgeNum].length;
 393   // normal
 394   Vector2 n = E.getPerpendicular();
 395   if (hitPt) *hitPt = Vector2::Zero;
 396   if (norm) *norm = Vector2::Zero;
 397   if (edgeD) *edgeD = 0.0f;
 398   // calculate the distance!
 399   float x = toP.dotProduct(E);
 400   if (x <= 0.0f) {
 401     // x is outside the line segment, distance is from pt to ptA
 402     dist = (pt-ptA).lengthSquared();
 403     //Vector2.DistanceSquared(ref pt, ref ptA, out dist);
 404     if (hitPt) *hitPt = ptA;
 405     if (edgeD) *edgeD = 0.0f;
 406     if (norm) *norm = n;
 407     //printf("getClosestPointonEdgeSquared - closest is ptA: %f\n", dist);
 408   } else if (x >= edgeLength) {
 409     // x is outside of the line segment, distance is from pt to ptB
 410     dist = (pt-ptB).lengthSquared();
 411     //Vector2.DistanceSquared(ref pt, ref ptB, out dist);
 412     if (hitPt) *hitPt = ptB;
 413     if (edgeD) *edgeD = 1.0f;
 414     if (norm) *norm = n;
 415     //printf("getClosestPointonEdgeSquared - closest is ptB: %f\n", dist);
 416   } else {
 417     // point lies somewhere on the line segment
 418     dist = toP.crossProduct(E);
 419     //Vector3.Cross(ref toP3, ref E3, out E3);
 420     dist *= dist;
 421     if (hitPt) *hitPt = ptA+(E*x);
 422     if (edgeD) *edgeD = x/edgeLength;
 423     if (norm) *norm = n;
 424     //printf("getClosestPointonEdgeSquared - closest is at %f: %f\n", edgeD, dist);
 425   }
 426   return dist;
 427 }
 428
 429
 430 int Body::getClosestPointMass (const Vector2 &pos, float *dist) const {
 431   float closestSQD = 100000.0f;
 432   int closest = -1, c = mPointCount;
 433   for (int i = 0; i < c-1; ++i) {
 434     float thisD = (pos-mPointMasses[i].Position).lengthSquared();
 435     if (thisD < closestSQD) {
 436       closestSQD = thisD;
 437       closest = i;
 438     }
 439   }
 440   if (dist) *dist = sqrtf(closestSQD);
 441   return closest;
 442 }
 443
 444
 445 void Body::addGlobalForce (const Vector2 &pt, const Vector2 &force) {
 446   Vector2 R = mDerivedPos-pt;
 447   float torqueF = R.crossProduct(force);
 448   for (PointMassList::iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it) {
 449     Vector2 toPt = (*it).Position-mDerivedPos;
 450     Vector2 torque = rotateVector(toPt, -HALF_PI);
 451     (*it).Force += torque*torqueF;
 452     (*it).Force += force;
 453   }
 454 }
 455
 456
 457 Vector2List Body::getVerticiesInWorld () const {
 458   Vector2List res;
 459   for (unsigned int f = mPointMasses.size(); f > 0; --f) res.push_back(Vector2::Zero);
 460   getVerticiesInWorld(res);
 461   return res;
 462 }
 463
 464
 465 void Body::getVerticiesInWorld (Vector2List &outList) const {
 466   Vector2List::iterator out = outList.begin();
 467   for (PointMassList::const_iterator it = mPointMasses.begin(); it != mPointMasses.end(); ++it, ++out) {
 468     *out = it->Position;
 469   }
 470 }
 471
 472
 473 }