src/jelly/Body.cpp

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