@box2d/debug-draw
Version:
Debug drawing helper for @box2d
193 lines (192 loc) • 8.13 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2AreaJoint = exports.b2AreaJointDef = void 0;
// Copyright (c) 2019 Erin Catto
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
// DEBUG: import { b2Assert } from "../common/b2_common";
const b2_common_1 = require("../common/b2_common");
const b2_math_1 = require("../common/b2_math");
const b2_joint_1 = require("./b2_joint");
const b2_distance_joint_1 = require("./b2_distance_joint");
class b2AreaJointDef extends b2_joint_1.b2JointDef {
constructor() {
super(b2_joint_1.b2JointType.e_areaJoint);
this.bodies = [];
this.stiffness = 0;
this.damping = 0;
}
AddBody(body) {
this.bodies.push(body);
if (this.bodies.length === 1) {
this.bodyA = body;
}
else if (this.bodies.length === 2) {
this.bodyB = body;
}
}
}
exports.b2AreaJointDef = b2AreaJointDef;
class b2AreaJoint extends b2_joint_1.b2Joint {
constructor(def) {
var _a, _b;
super(def);
this.m_stiffness = 0;
this.m_damping = 0;
// Solver shared
this.m_impulse = 0;
this.m_targetArea = 0;
this.m_joints = [];
this.m_delta = new b2_math_1.b2Vec2();
// DEBUG: b2Assert(def.bodies.length >= 3, "You cannot create an area joint with less than three bodies.");
this.m_bodies = def.bodies;
this.m_stiffness = (_a = def.stiffness) !== null && _a !== void 0 ? _a : 0;
this.m_damping = (_b = def.damping) !== null && _b !== void 0 ? _b : 0;
this.m_targetLengths = (0, b2_common_1.b2MakeNumberArray)(def.bodies.length);
this.m_normals = (0, b2_common_1.b2MakeArray)(def.bodies.length, b2_math_1.b2Vec2);
this.m_deltas = (0, b2_common_1.b2MakeArray)(def.bodies.length, b2_math_1.b2Vec2);
const djd = new b2_distance_joint_1.b2DistanceJointDef();
djd.stiffness = this.m_stiffness;
djd.damping = this.m_damping;
this.m_targetArea = 0;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const next = this.m_bodies[(i + 1) % this.m_bodies.length];
const body_c = body.GetWorldCenter();
const next_c = next.GetWorldCenter();
this.m_targetLengths[i] = b2_math_1.b2Vec2.Distance(body_c, next_c);
this.m_targetArea += b2_math_1.b2Vec2.Cross(body_c, next_c);
djd.Initialize(body, next, body_c, next_c);
this.m_joints[i] = body.GetWorld().CreateJoint(djd);
}
this.m_targetArea *= 0.5;
}
GetAnchorA(out) {
return out;
}
GetAnchorB(out) {
return out;
}
GetReactionForce(inv_dt, out) {
return out;
}
GetReactionTorque(_inv_dt) {
return 0;
}
SetStiffness(stiffness) {
this.m_stiffness = stiffness;
for (const joint of this.m_joints) {
joint.SetStiffness(stiffness);
}
}
GetStiffness() {
return this.m_stiffness;
}
SetDamping(damping) {
this.m_damping = damping;
for (const joint of this.m_joints) {
joint.SetDamping(damping);
}
}
GetDamping() {
return this.m_damping;
}
InitVelocityConstraints(data) {
for (let i = 0; i < this.m_bodies.length; ++i) {
const prev = this.m_bodies[(i + this.m_bodies.length - 1) % this.m_bodies.length];
const next = this.m_bodies[(i + 1) % this.m_bodies.length];
const prev_c = data.positions[prev.m_islandIndex].c;
const next_c = data.positions[next.m_islandIndex].c;
const delta = this.m_deltas[i];
b2_math_1.b2Vec2.Subtract(next_c, prev_c, delta);
}
if (data.step.warmStarting) {
this.m_impulse *= data.step.dtRatio;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const body_v = data.velocities[body.m_islandIndex].v;
const delta = this.m_deltas[i];
body_v.x += body.m_invMass * delta.y * 0.5 * this.m_impulse;
body_v.y += body.m_invMass * -delta.x * 0.5 * this.m_impulse;
}
}
else {
this.m_impulse = 0;
}
}
SolveVelocityConstraints(data) {
let dotMassSum = 0;
let crossMassSum = 0;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const body_v = data.velocities[body.m_islandIndex].v;
const delta = this.m_deltas[i];
dotMassSum += delta.LengthSquared() / body.GetMass();
crossMassSum += b2_math_1.b2Vec2.Cross(body_v, delta);
}
const lambda = (-2 * crossMassSum) / dotMassSum;
// lambda = b2Clamp(lambda, -b2_maxLinearCorrection, b2_maxLinearCorrection);
this.m_impulse += lambda;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const body_v = data.velocities[body.m_islandIndex].v;
const delta = this.m_deltas[i];
body_v.x += body.m_invMass * delta.y * 0.5 * lambda;
body_v.y += body.m_invMass * -delta.x * 0.5 * lambda;
}
}
SolvePositionConstraints(data) {
let perimeter = 0;
let area = 0;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const next = this.m_bodies[(i + 1) % this.m_bodies.length];
const body_c = data.positions[body.m_islandIndex].c;
const next_c = data.positions[next.m_islandIndex].c;
const delta = b2_math_1.b2Vec2.Subtract(next_c, body_c, this.m_delta);
let dist = delta.Length();
if (dist < b2_common_1.b2_epsilon) {
dist = 1;
}
this.m_normals[i].x = delta.y / dist;
this.m_normals[i].y = -delta.x / dist;
perimeter += dist;
area += b2_math_1.b2Vec2.Cross(body_c, next_c);
}
area *= 0.5;
const deltaArea = this.m_targetArea - area;
const toExtrude = (0.5 * deltaArea) / perimeter;
let done = true;
for (let i = 0; i < this.m_bodies.length; ++i) {
const body = this.m_bodies[i];
const body_c = data.positions[body.m_islandIndex].c;
const next_i = (i + 1) % this.m_bodies.length;
const delta = b2_math_1.b2Vec2.Add(this.m_normals[i], this.m_normals[next_i], this.m_delta);
delta.Scale(toExtrude);
const norm_sq = delta.LengthSquared();
if (norm_sq > b2_common_1.b2_maxLinearCorrection ** 2) {
delta.Scale(b2_common_1.b2_maxLinearCorrection / Math.sqrt(norm_sq));
}
if (norm_sq > b2_common_1.b2_linearSlop ** 2) {
done = false;
}
body_c.Add(delta);
}
return done;
}
}
exports.b2AreaJoint = b2AreaJoint;