static void preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) { CONSTRAINT_BEGIN(spring, a, b); cpFloat moment = a->i_inv + b->i_inv; spring->iSum = 1.0f/moment; spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); spring->target_wrn = 0.0f; // apply spring torque cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; a->w -= j_spring*a->i_inv; b->w += j_spring*b->i_inv; }
static void preStep(cpDampedRotarySpring *spring, cpFloat dt) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; cpFloat moment = a->i_inv + b->i_inv; cpAssertSoft(moment != 0.0, "Unsolvable spring."); spring->iSum = 1.0f/moment; spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment); spring->target_wrn = 0.0f; // apply spring torque cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; a->w -= j_spring*a->i_inv; b->w += j_spring*b->i_inv; }
static void applyImpulse(cpDampedRotarySpring *spring) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; // compute relative velocity cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; // compute velocity loss from drag // not 100% certain this is derived correctly, though it makes sense cpFloat w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum)); spring->target_wrn = wrn - w_damp; //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); cpFloat j_damp = w_damp*spring->iSum; a->w -= j_damp*a->i_inv; b->w += j_damp*b->i_inv; }
static void applyImpulse(cpDampedSpring *spring) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; cpVect n = spring->n; cpVect r1 = spring->r1; cpVect r2 = spring->r2; // compute relative velocity cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; // compute velocity loss from drag // not 100% certain this is derived correctly, though it makes sense cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass)); spring->target_vrn = vrn + v_damp; apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); }
static void preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) { CONSTRAINT_BEGIN(spring, a, b); spring->r1 = cpvrotate(spring->anchr1, a->rot); spring->r2 = cpvrotate(spring->anchr2, b->rot); cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); cpFloat dist = cpvlength(delta); spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); spring->nMass = 1.0f/k; spring->target_vrn = 0.0f; spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); // apply spring force cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt)); }
static void preStep(cpDampedSpring *spring, cpFloat dt) { cpBody *a = spring->constraint.a; cpBody *b = spring->constraint.b; spring->r1 = cpvrotate(spring->anchr1, a->rot); spring->r2 = cpvrotate(spring->anchr2, b->rot); cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); cpFloat dist = cpvlength(delta); spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n); cpAssertSoft(k != 0.0, "Unsolvable spring."); spring->nMass = 1.0f/k; spring->target_vrn = 0.0f; spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k); // apply spring force cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt)); }
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr) { CP_ARBITER_GET_SHAPES(arb, water, poly); cpBody *body = cpShapeGetBody(poly); // Get the top of the water sensor bounding box to use as the water level. cpFloat level = cpShapeGetBB(water).t; // Clip the polygon against the water level int count = cpPolyShapeGetCount(poly); int clippedCount = 0; #ifdef _MSC_VER // MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays. cpVect clipped[10]; #else cpVect clipped[count + 1]; #endif for(int i=0, j=count-1; i<count; j=i, i++){ cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j)); cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i)); if(a.y < level){ clipped[clippedCount] = a; clippedCount++; } cpFloat a_level = a.y - level; cpFloat b_level = b.y - level; if(a_level*b_level < 0.0f){ cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level)); clipped[clippedCount] = cpvlerp(a, b, t); clippedCount++; } } // Calculate buoyancy from the clipped polygon area cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f); cpFloat displacedMass = clippedArea*FLUID_DENSITY; cpVect centroid = cpCentroidForPoly(clippedCount, clipped); cpDataPointer data = ptr; DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data); DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data); cpFloat dt = cpSpaceGetCurrentTimeStep(space); cpVect g = cpSpaceGetGravity(space); // Apply the buoyancy force as an impulse. cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid); // Apply linear damping for the fluid drag. cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid); cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid)); cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY; cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag // cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid); // Apply angular damping for the fluid drag. cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body)); cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f); cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body))); return cpTrue; }