@box2d/debug-draw
Version:
Debug drawing helper for @box2d
1,143 lines (1,142 loc) • 29.4 kB
JavaScript
"use strict";
// MIT License
Object.defineProperty(exports, "__esModule", { value: true });
exports.b2Sweep = exports.b2Transform = exports.b2Rot = exports.b2Mat33 = exports.b2Mat22 = exports.b2Vec3 = exports.b2Vec2 = exports.b2RandomInt = exports.b2RandomFloat = exports.b2Random = exports.b2IsPowerOfTwo = exports.b2NextPowerOfTwo = exports.b2RadToDeg = exports.b2DegToRad = exports.b2Clamp = exports.b2_two_pi = exports.b2_180_over_pi = exports.b2_pi_over_180 = void 0;
const b2_common_1 = require("./b2_common");
exports.b2_pi_over_180 = Math.PI / 180;
exports.b2_180_over_pi = 180 / Math.PI;
exports.b2_two_pi = 2 * Math.PI;
function b2Clamp(a, low, high) {
if (a < low)
return low;
return a > high ? high : a;
}
exports.b2Clamp = b2Clamp;
function b2DegToRad(degrees) {
return degrees * exports.b2_pi_over_180;
}
exports.b2DegToRad = b2DegToRad;
function b2RadToDeg(radians) {
return radians * exports.b2_180_over_pi;
}
exports.b2RadToDeg = b2RadToDeg;
/**
* "Next Largest Power of 2
* Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
* that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
* the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
* largest power of 2. For a 32-bit value:"
*/
function b2NextPowerOfTwo(x) {
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
x |= x >> 8;
x |= x >> 16;
return x + 1;
}
exports.b2NextPowerOfTwo = b2NextPowerOfTwo;
function b2IsPowerOfTwo(x) {
return x > 0 && (x & (x - 1)) === 0;
}
exports.b2IsPowerOfTwo = b2IsPowerOfTwo;
function b2Random() {
return Math.random() * 2 - 1;
}
exports.b2Random = b2Random;
function b2RandomFloat(lo, hi) {
return (hi - lo) * Math.random() + lo;
}
exports.b2RandomFloat = b2RandomFloat;
function b2RandomInt(lo, hi) {
return Math.round((hi - lo) * Math.random() + lo);
}
exports.b2RandomInt = b2RandomInt;
/**
* A 2D column vector.
*/
class b2Vec2 {
constructor(x = 0, y = 0) {
this.x = x;
this.y = y;
}
Clone() {
return new b2Vec2(this.x, this.y);
}
/**
* Set this vector to all zeros.
*/
SetZero() {
this.x = 0;
this.y = 0;
return this;
}
/**
* Set this vector to some specified coordinates.
*/
Set(x, y) {
this.x = x;
this.y = y;
return this;
}
Copy(other) {
this.x = other.x;
this.y = other.y;
return this;
}
/**
* Add a vector to this vector.
*/
Add(v) {
this.x += v.x;
this.y += v.y;
return this;
}
/**
* Add a vector to this vector.
*/
AddXY(x, y) {
this.x += x;
this.y += y;
return this;
}
/**
* Subtract a vector from this vector.
*/
Subtract(v) {
this.x -= v.x;
this.y -= v.y;
return this;
}
/**
* Subtract a vector from this vector.
*/
SubtractXY(x, y) {
this.x -= x;
this.y -= y;
return this;
}
/**
* Multiply this vector by a scalar.
*/
Scale(s) {
this.x *= s;
this.y *= s;
return this;
}
AddScaled(s, v) {
this.x += s * v.x;
this.y += s * v.y;
return this;
}
SubtractScaled(s, v) {
this.x -= s * v.x;
this.y -= s * v.y;
return this;
}
/**
* Perform the dot product on two vectors.
*/
Dot(v) {
return this.x * v.x + this.y * v.y;
}
/**
* Perform the cross product on two vectors. In 2D this produces a scalar.
*/
Cross(v) {
return this.x * v.y - this.y * v.x;
}
/**
* Get the length of this vector (the norm).
*/
Length() {
const { x, y } = this;
return Math.sqrt(x * x + y * y);
}
/**
* Get the length squared. For performance, use this instead of
* b2Vec2::Length (if possible).
*/
LengthSquared() {
const { x, y } = this;
return x * x + y * y;
}
/**
* Convert this vector into a unit vector. Returns the length.
*/
Normalize() {
const length = this.Length();
if (length < b2_common_1.b2_epsilon) {
return 0;
}
const inv_length = 1 / length;
this.x *= inv_length;
this.y *= inv_length;
return length;
}
Rotate(radians) {
const c = Math.cos(radians);
const s = Math.sin(radians);
const { x } = this;
this.x = c * x - s * this.y;
this.y = s * x + c * this.y;
return this;
}
RotateCosSin(c, s) {
const { x } = this;
this.x = c * x - s * this.y;
this.y = s * x + c * this.y;
return this;
}
/**
* Does this vector contain finite coordinates?
*/
IsValid() {
return Number.isFinite(this.x) && Number.isFinite(this.y);
}
Abs() {
this.x = Math.abs(this.x);
this.y = Math.abs(this.y);
return this;
}
GetAbs(out) {
out.x = Math.abs(this.x);
out.y = Math.abs(this.y);
return out;
}
/**
* Negate this vector.
*/
Negate() {
this.x = -this.x;
this.y = -this.y;
return this;
}
/**
* Skew this vector such that dot(skew_vec, other) == cross(vec, other)
*/
Skew() {
const { x } = this;
this.x = -this.y;
this.y = x;
return this;
}
static Min(a, b, out) {
out.x = Math.min(a.x, b.x);
out.y = Math.min(a.y, b.y);
return out;
}
static Max(a, b, out) {
out.x = Math.max(a.x, b.x);
out.y = Math.max(a.y, b.y);
return out;
}
static Clamp(v, lo, hi, out) {
out.x = b2Clamp(v.x, lo.x, hi.x);
out.y = b2Clamp(v.y, lo.y, hi.y);
return out;
}
static Rotate(v, radians, out) {
const v_x = v.x;
const v_y = v.y;
const c = Math.cos(radians);
const s = Math.sin(radians);
out.x = c * v_x - s * v_y;
out.y = s * v_x + c * v_y;
return out;
}
/** Perform the dot product on two vectors. */
static Dot(a, b) {
return a.x * b.x + a.y * b.y;
}
/** Perform the cross product on two vectors. In 2D this produces a scalar. */
static Cross(a, b) {
return a.x * b.y - a.y * b.x;
}
/**
* Perform the cross product on a vector and a scalar. In 2D this produces
* a vector.
*/
static CrossVec2Scalar(v, s, out) {
const v_x = v.x;
out.x = s * v.y;
out.y = -s * v_x;
return out;
}
static CrossVec2One(v, out) {
const v_x = v.x;
out.x = v.y;
out.y = -v_x;
return out;
}
/**
* Perform the cross product on a scalar and a vector. In 2D this produces
* a vector.
*/
static CrossScalarVec2(s, v, out) {
const v_x = v.x;
out.x = -s * v.y;
out.y = s * v_x;
return out;
}
static CrossOneVec2(v, out) {
const v_x = v.x;
out.x = -v.y;
out.y = v_x;
return out;
}
/**
* Add two vectors component-wise.
*/
static Add(a, b, out) {
out.x = a.x + b.x;
out.y = a.y + b.y;
return out;
}
/**
* Subtract two vectors component-wise.
*/
static Subtract(a, b, out) {
out.x = a.x - b.x;
out.y = a.y - b.y;
return out;
}
static Scale(s, v, out) {
out.x = v.x * s;
out.y = v.y * s;
return out;
}
static AddScaled(a, s, b, out) {
out.x = a.x + s * b.x;
out.y = a.y + s * b.y;
return out;
}
static SubtractScaled(a, s, b, out) {
out.x = a.x - s * b.x;
out.y = a.y - s * b.y;
return out;
}
static AddCrossScalarVec2(a, s, v, out) {
const v_x = v.x;
out.x = a.x - s * v.y;
out.y = a.y + s * v_x;
return out;
}
static Mid(a, b, out) {
out.x = (a.x + b.x) * 0.5;
out.y = (a.y + b.y) * 0.5;
return out;
}
static Extents(a, b, out) {
out.x = (b.x - a.x) * 0.5;
out.y = (b.y - a.y) * 0.5;
return out;
}
static Equals(a, b) {
return a.x === b.x && a.y === b.y;
}
static Distance(a, b) {
return Math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2);
}
static DistanceSquared(a, b) {
return (a.x - b.x) ** 2 + (a.y - b.y) ** 2;
}
/**
* Negate a vector.
*/
static Negate(v, out) {
out.x = -v.x;
out.y = -v.y;
return out;
}
static Normalize(v, out) {
const length_sq = v.x ** 2 + v.y ** 2;
if (length_sq >= b2_common_1.b2_epsilon_sq) {
const inv_length = 1 / Math.sqrt(length_sq);
out.x = inv_length * v.x;
out.y = inv_length * v.y;
}
else {
out.x = 0;
out.y = 0;
}
return out;
}
/**
* Skew a vector such that dot(skew_vec, other) == cross(vec, other)
*/
static Skew(v, out) {
const { x } = v;
out.x = -v.y;
out.y = x;
return out;
}
}
exports.b2Vec2 = b2Vec2;
b2Vec2.ZERO = new b2Vec2();
b2Vec2.UNITX = new b2Vec2(1, 0);
b2Vec2.UNITY = new b2Vec2(0, 1);
b2Vec2.s_t0 = new b2Vec2();
b2Vec2.s_t1 = new b2Vec2();
b2Vec2.s_t2 = new b2Vec2();
b2Vec2.s_t3 = new b2Vec2();
/**
* A 2D column vector with 3 elements.
*/
class b2Vec3 {
constructor(x = 0, y = 0, z = 0) {
this.x = x;
this.y = y;
this.z = z;
}
Clone() {
return new b2Vec3(this.x, this.y, this.z);
}
/**
* Set this vector to all zeros.
*/
SetZero() {
this.x = 0;
this.y = 0;
this.z = 0;
return this;
}
/**
* Set this vector to some specified coordinates.
*/
Set(x, y, z) {
this.x = x;
this.y = y;
this.z = z;
return this;
}
Copy(other) {
this.x = other.x;
this.y = other.y;
this.z = other.z;
return this;
}
/**
* Negate this vector.
*/
Negate() {
this.x = -this.x;
this.y = -this.y;
this.z = -this.z;
return this;
}
/**
* Add a vector to this vector.
*/
Add(v) {
this.x += v.x;
this.y += v.y;
this.z += v.z;
return this;
}
/**
* Add a vector to this vector.
*/
AddXYZ(x, y, z) {
this.x += x;
this.y += y;
this.z += z;
return this;
}
/**
* Subtract a vector from this vector.
*/
Subtract(v) {
this.x -= v.x;
this.y -= v.y;
this.z -= v.z;
return this;
}
/**
* Subtract a vector from this vector.
*/
SubtractXYZ(x, y, z) {
this.x -= x;
this.y -= y;
this.z -= z;
return this;
}
/**
* Multiply this vector by a scalar.
*/
Scale(s) {
this.x *= s;
this.y *= s;
this.z *= s;
return this;
}
/**
* Perform the dot product on two vectors.
*/
static Dot(a, b) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/**
* Perform the cross product on two vectors.
*/
static Cross(a, b, out) {
const a_x = a.x;
const a_y = a.y;
const a_z = a.z;
const b_x = b.x;
const b_y = b.y;
const b_z = b.z;
out.x = a_y * b_z - a_z * b_y;
out.y = a_z * b_x - a_x * b_z;
out.z = a_x * b_y - a_y * b_x;
return out;
}
}
exports.b2Vec3 = b2Vec3;
b2Vec3.ZERO = new b2Vec3(0, 0, 0);
b2Vec3.s_t0 = new b2Vec3();
/**
* A 2-by-2 matrix. Stored in column-major order.
*/
class b2Mat22 {
constructor() {
this.ex = new b2Vec2(1, 0);
this.ey = new b2Vec2(0, 1);
}
Clone() {
return new b2Mat22().Copy(this);
}
/**
* Construct a matrix using columns.
*/
static FromColumns(c1, c2) {
return new b2Mat22().SetColumns(c1, c2);
}
/**
* Construct a matrix using scalars.
*/
static FromScalars(r1c1, r1c2, r2c1, r2c2) {
return new b2Mat22().SetScalars(r1c1, r1c2, r2c1, r2c2);
}
static FromAngle(radians) {
return new b2Mat22().SetAngle(radians);
}
/**
* Set this matrix using scalars.
*/
SetScalars(r1c1, r1c2, r2c1, r2c2) {
this.ex.Set(r1c1, r2c1);
this.ey.Set(r1c2, r2c2);
return this;
}
/**
* Initialize this matrix using columns.
*/
SetColumns(c1, c2) {
this.ex.Copy(c1);
this.ey.Copy(c2);
return this;
}
SetAngle(radians) {
const c = Math.cos(radians);
const s = Math.sin(radians);
this.ex.Set(c, s);
this.ey.Set(-s, c);
return this;
}
Copy(other) {
this.ex.Copy(other.ex);
this.ey.Copy(other.ey);
return this;
}
/**
* Set this to the identity matrix.
*/
SetIdentity() {
this.ex.Set(1, 0);
this.ey.Set(0, 1);
return this;
}
/**
* Set this matrix to all zeros.
*/
SetZero() {
this.ex.SetZero();
this.ey.SetZero();
return this;
}
GetAngle() {
return Math.atan2(this.ex.y, this.ex.x);
}
/**
* Solve A * x = b, where b is a column vector. This is more efficient
* than computing the inverse in one-shot cases.
*/
Solve(b_x, b_y, out) {
const a11 = this.ex.x;
const a12 = this.ey.x;
const a21 = this.ex.y;
const a22 = this.ey.y;
let det = a11 * a22 - a12 * a21;
if (det !== 0) {
det = 1 / det;
}
out.x = det * (a22 * b_x - a12 * b_y);
out.y = det * (a11 * b_y - a21 * b_x);
return out;
}
Abs() {
this.ex.Abs();
this.ey.Abs();
return this;
}
Inverse() {
this.GetInverse(this);
return this;
}
Add(M) {
this.ex.Add(M.ex);
this.ey.Add(M.ey);
return this;
}
Subtract(M) {
this.ex.Subtract(M.ex);
this.ey.Subtract(M.ey);
return this;
}
GetInverse(out) {
const a = this.ex.x;
const b = this.ey.x;
const c = this.ex.y;
const d = this.ey.y;
let det = a * d - b * c;
if (det !== 0) {
det = 1 / det;
}
out.ex.x = det * d;
out.ey.x = -det * b;
out.ex.y = -det * c;
out.ey.y = det * a;
return out;
}
GetAbs(out) {
out.ex.x = Math.abs(this.ex.x);
out.ex.y = Math.abs(this.ex.y);
out.ey.x = Math.abs(this.ey.x);
out.ey.y = Math.abs(this.ey.y);
return out;
}
/**
* Multiply a matrix times a vector. If a rotation matrix is provided,
* then this transforms the vector from one frame to another.
*/
static MultiplyVec2(M, v, out) {
const v_x = v.x;
const v_y = v.y;
out.x = M.ex.x * v_x + M.ey.x * v_y;
out.y = M.ex.y * v_x + M.ey.y * v_y;
return out;
}
/**
* Multiply a matrix transpose times a vector. If a rotation matrix is provided,
* then this transforms the vector from one frame to another (inverse transform).
*/
static TransposeMultiplyVec2(M, v, out) {
const v_x = v.x;
const v_y = v.y;
out.x = M.ex.x * v_x + M.ex.y * v_y;
out.y = M.ey.x * v_x + M.ey.y * v_y;
return out;
}
static Add(A, B, out) {
out.ex.x = A.ex.x + B.ex.x;
out.ex.y = A.ex.y + B.ex.y;
out.ey.x = A.ey.x + B.ey.x;
out.ey.y = A.ey.y + B.ey.y;
return out;
}
/** A * B */
static Multiply(A, B, out) {
const A_ex_x = A.ex.x;
const A_ex_y = A.ex.y;
const A_ey_x = A.ey.x;
const A_ey_y = A.ey.y;
const B_ex_x = B.ex.x;
const B_ex_y = B.ex.y;
const B_ey_x = B.ey.x;
const B_ey_y = B.ey.y;
out.ex.x = A_ex_x * B_ex_x + A_ey_x * B_ex_y;
out.ex.y = A_ex_y * B_ex_x + A_ey_y * B_ex_y;
out.ey.x = A_ex_x * B_ey_x + A_ey_x * B_ey_y;
out.ey.y = A_ex_y * B_ey_x + A_ey_y * B_ey_y;
return out;
}
/** A^T * B */
static TransposeMultiply(A, B, out) {
const A_ex_x = A.ex.x;
const A_ex_y = A.ex.y;
const A_ey_x = A.ey.x;
const A_ey_y = A.ey.y;
const B_ex_x = B.ex.x;
const B_ex_y = B.ex.y;
const B_ey_x = B.ey.x;
const B_ey_y = B.ey.y;
out.ex.x = A_ex_x * B_ex_x + A_ex_y * B_ex_y;
out.ex.y = A_ey_x * B_ex_x + A_ey_y * B_ex_y;
out.ey.x = A_ex_x * B_ey_x + A_ex_y * B_ey_y;
out.ey.y = A_ey_x * B_ey_x + A_ey_y * B_ey_y;
return out;
}
}
exports.b2Mat22 = b2Mat22;
b2Mat22.IDENTITY = new b2Mat22();
/**
* A 3-by-3 matrix. Stored in column-major order.
*/
class b2Mat33 {
constructor() {
this.ex = new b2Vec3(1, 0, 0);
this.ey = new b2Vec3(0, 1, 0);
this.ez = new b2Vec3(0, 0, 1);
}
Clone() {
return new b2Mat33().Copy(this);
}
/**
* Set this matrix using columns.
*/
SetColumns(c1, c2, c3) {
this.ex.Copy(c1);
this.ey.Copy(c2);
this.ez.Copy(c3);
return this;
}
Copy(other) {
this.ex.Copy(other.ex);
this.ey.Copy(other.ey);
this.ez.Copy(other.ez);
return this;
}
SetIdentity() {
this.ex.Set(1, 0, 0);
this.ey.Set(0, 1, 0);
this.ez.Set(0, 0, 1);
return this;
}
/**
* Set this matrix to all zeros.
*/
SetZero() {
this.ex.SetZero();
this.ey.SetZero();
this.ez.SetZero();
return this;
}
Add(M) {
this.ex.Add(M.ex);
this.ey.Add(M.ey);
this.ez.Add(M.ez);
return this;
}
/**
* Solve A * x = b, where b is a column vector. This is more efficient
* than computing the inverse in one-shot cases.
*/
Solve33(b_x, b_y, b_z, out) {
const a11 = this.ex.x;
const a21 = this.ex.y;
const a31 = this.ex.z;
const a12 = this.ey.x;
const a22 = this.ey.y;
const a32 = this.ey.z;
const a13 = this.ez.x;
const a23 = this.ez.y;
const a33 = this.ez.z;
let det = a11 * (a22 * a33 - a32 * a23) + a21 * (a32 * a13 - a12 * a33) + a31 * (a12 * a23 - a22 * a13);
if (det !== 0) {
det = 1 / det;
}
out.x = det * (b_x * (a22 * a33 - a32 * a23) + b_y * (a32 * a13 - a12 * a33) + b_z * (a12 * a23 - a22 * a13));
out.y = det * (a11 * (b_y * a33 - b_z * a23) + a21 * (b_z * a13 - b_x * a33) + a31 * (b_x * a23 - b_y * a13));
out.z = det * (a11 * (a22 * b_z - a32 * b_y) + a21 * (a32 * b_x - a12 * b_z) + a31 * (a12 * b_y - a22 * b_x));
return out;
}
/**
* Solve A * x = b, where b is a column vector. This is more efficient
* than computing the inverse in one-shot cases. Solve only the upper
* 2-by-2 matrix equation.
*/
Solve22(b_x, b_y, out) {
const a11 = this.ex.x;
const a12 = this.ey.x;
const a21 = this.ex.y;
const a22 = this.ey.y;
let det = a11 * a22 - a12 * a21;
if (det !== 0) {
det = 1 / det;
}
out.x = det * (a22 * b_x - a12 * b_y);
out.y = det * (a11 * b_y - a21 * b_x);
return out;
}
/**
* Get the inverse of this matrix as a 2-by-2.
* Returns the zero matrix if singular.
*/
GetInverse22(M) {
const a = this.ex.x;
const b = this.ey.x;
const c = this.ex.y;
const d = this.ey.y;
let det = a * d - b * c;
if (det !== 0) {
det = 1 / det;
}
M.ex.x = det * d;
M.ey.x = -det * b;
M.ex.z = 0;
M.ex.y = -det * c;
M.ey.y = det * a;
M.ey.z = 0;
M.ez.x = 0;
M.ez.y = 0;
M.ez.z = 0;
}
/**
* Get the symmetric inverse of this matrix as a 3-by-3.
* Returns the zero matrix if singular.
*/
GetSymInverse33(M) {
let det = b2Vec3.Dot(this.ex, b2Vec3.Cross(this.ey, this.ez, b2Vec3.s_t0));
if (det !== 0) {
det = 1 / det;
}
const a11 = this.ex.x;
const a12 = this.ey.x;
const a13 = this.ez.x;
const a22 = this.ey.y;
const a23 = this.ez.y;
const a33 = this.ez.z;
M.ex.x = det * (a22 * a33 - a23 * a23);
M.ex.y = det * (a13 * a23 - a12 * a33);
M.ex.z = det * (a12 * a23 - a13 * a22);
M.ey.x = M.ex.y;
M.ey.y = det * (a11 * a33 - a13 * a13);
M.ey.z = det * (a13 * a12 - a11 * a23);
M.ez.x = M.ex.z;
M.ez.y = M.ey.z;
M.ez.z = det * (a11 * a22 - a12 * a12);
}
/**
* Multiply a matrix times a vector.
*/
static MultiplyVec3(A, v, out) {
const { x, y, z } = v;
out.x = A.ex.x * x + A.ey.x * y + A.ez.x * z;
out.y = A.ex.y * x + A.ey.y * y + A.ez.y * z;
out.z = A.ex.z * x + A.ey.z * y + A.ez.z * z;
return out;
}
/**
* Multiply a matrix times a vector.
*/
static MultiplyVec2(A, v, out) {
const { x, y } = v;
out.x = A.ex.x * x + A.ey.x * y;
out.y = A.ex.y * x + A.ey.y * y;
return out;
}
}
exports.b2Mat33 = b2Mat33;
b2Mat33.IDENTITY = new b2Mat33();
/**
* Rotation
*/
class b2Rot {
/**
* Initialize from an angle in radians
*/
constructor(angle = 0) {
/** Sine */
this.s = 0;
/** Cosine */
this.c = 1;
if (angle) {
this.s = Math.sin(angle);
this.c = Math.cos(angle);
}
}
Clone() {
return new b2Rot().Copy(this);
}
Copy(other) {
this.s = other.s;
this.c = other.c;
return this;
}
/**
* Set using an angle in radians.
*/
Set(angle) {
this.s = Math.sin(angle);
this.c = Math.cos(angle);
return this;
}
/**
* Set to the identity rotation
*/
SetIdentity() {
this.s = 0;
this.c = 1;
return this;
}
/**
* Get the angle in radians
*/
GetAngle() {
return Math.atan2(this.s, this.c);
}
/**
* Get the x-axis
*/
GetXAxis(out) {
out.x = this.c;
out.y = this.s;
return out;
}
/**
* Get the u-axis
*/
GetYAxis(out) {
out.x = -this.s;
out.y = this.c;
return out;
}
/**
* Multiply two rotations: q * r
*/
static Multiply(q, r, out) {
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
// s = qs * rc + qc * rs
// c = qc * rc - qs * rs
const s = q.s * r.c + q.c * r.s;
const c = q.c * r.c - q.s * r.s;
out.s = s;
out.c = c;
return out;
}
/**
* Transpose multiply two rotations: qT * r
*/
static TransposeMultiply(q, r, out) {
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
const s = q.c * r.s - q.s * r.c;
const c = q.c * r.c + q.s * r.s;
out.s = s;
out.c = c;
return out;
}
/**
* Rotate a vector
*/
static MultiplyVec2(q, v, out) {
const v_x = v.x;
const v_y = v.y;
out.x = q.c * v_x - q.s * v_y;
out.y = q.s * v_x + q.c * v_y;
return out;
}
/**
* Inverse rotate a vector
*/
static TransposeMultiplyVec2(q, v, out) {
const v_x = v.x;
const v_y = v.y;
out.x = q.c * v_x + q.s * v_y;
out.y = -q.s * v_x + q.c * v_y;
return out;
}
}
exports.b2Rot = b2Rot;
b2Rot.IDENTITY = new b2Rot();
/**
* A transform contains translation and rotation. It is used to represent
* the position and orientation of rigid frames.
*/
class b2Transform {
constructor() {
this.p = new b2Vec2();
this.q = new b2Rot();
}
Clone() {
return new b2Transform().Copy(this);
}
Copy(other) {
this.p.Copy(other.p);
this.q.Copy(other.q);
return this;
}
/**
* Set this to the identity transform.
*/
SetIdentity() {
this.p.SetZero();
this.q.SetIdentity();
return this;
}
/**
* Set this based on the position and rotation.
*/
SetPositionRotation(position, q) {
this.p.Copy(position);
this.q.Copy(q);
return this;
}
/**
* Set this based on the position and angle.
*/
SetPositionAngle(pos, a) {
this.p.Copy(pos);
this.q.Set(a);
return this;
}
SetPosition(position) {
this.p.Copy(position);
return this;
}
SetPositionXY(x, y) {
this.p.Set(x, y);
return this;
}
SetRotation(rotation) {
this.q.Copy(rotation);
return this;
}
SetRotationAngle(radians) {
this.q.Set(radians);
return this;
}
GetPosition() {
return this.p;
}
GetRotation() {
return this.q;
}
GetAngle() {
return this.q.GetAngle();
}
static MultiplyVec2(T, v, out) {
const v_x = v.x;
const v_y = v.y;
out.x = T.q.c * v_x - T.q.s * v_y + T.p.x;
out.y = T.q.s * v_x + T.q.c * v_y + T.p.y;
return out;
}
static TransposeMultiplyVec2(T, v, out) {
const px = v.x - T.p.x;
const py = v.y - T.p.y;
out.x = T.q.c * px + T.q.s * py;
out.y = -T.q.s * px + T.q.c * py;
return out;
}
/**
* v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
* = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
*/
static Multiply(A, B, out) {
b2Rot.Multiply(A.q, B.q, out.q);
b2Rot.MultiplyVec2(A.q, B.p, out.p).Add(A.p);
return out;
}
/**
* v2 = A.q' * (B.q * v1 + B.p - A.p)
* = A.q' * B.q * v1 + A.q' * (B.p - A.p)
*/
static TransposeMultiply(A, B, out) {
b2Rot.TransposeMultiply(A.q, B.q, out.q);
b2Rot.TransposeMultiplyVec2(A.q, b2Vec2.Subtract(B.p, A.p, out.p), out.p);
return out;
}
}
exports.b2Transform = b2Transform;
b2Transform.IDENTITY = new b2Transform();
/**
* This describes the motion of a body/shape for TOI computation.
* Shapes are defined with respect to the body origin, which may
* no coincide with the center of mass. However, to support dynamics
* we must interpolate the center of mass position.
*/
class b2Sweep {
constructor() {
/** Local center of mass position */
this.localCenter = new b2Vec2();
/** Center world position at time 0 */
this.c0 = new b2Vec2();
/** Center world position at time 1 */
this.c = new b2Vec2();
/** World angle at time 0 */
this.a0 = 0;
/** World angle at time 1 */
this.a = 0;
/**
* Fraction of the current time step in the range [0,1]
* c0 and a0 are the positions at alpha0.
*/
this.alpha0 = 0;
}
Clone() {
return new b2Sweep().Copy(this);
}
Copy(other) {
this.localCenter.Copy(other.localCenter);
this.c0.Copy(other.c0);
this.c.Copy(other.c);
this.a0 = other.a0;
this.a = other.a;
this.alpha0 = other.alpha0;
return this;
}
/**
* Get the interpolated transform at a specific time.
*
* @param transform The output transform
* @param beta Is a factor in [0,1], where 0 indicates alpha0.
* @see https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
*/
GetTransform(xf, beta) {
const oneMinusBeta = 1 - beta;
xf.p.x = oneMinusBeta * this.c0.x + beta * this.c.x;
xf.p.y = oneMinusBeta * this.c0.y + beta * this.c.y;
const angle = oneMinusBeta * this.a0 + beta * this.a;
xf.q.Set(angle);
// Shift to origin
xf.p.Subtract(b2Rot.MultiplyVec2(xf.q, this.localCenter, b2Vec2.s_t0));
return xf;
}
/**
* Advance the sweep forward, yielding a new initial state.
*
* @param alpha The new initial time.
*/
Advance(alpha) {
// DEBUG: b2Assert(this.alpha0 < 1);
const beta = (alpha - this.alpha0) / (1 - this.alpha0);
this.c0.x += beta * (this.c.x - this.c0.x);
this.c0.y += beta * (this.c.y - this.c0.y);
this.a0 += beta * (this.a - this.a0);
this.alpha0 = alpha;
}
/**
* Normalize an angle in radians to be between -pi and pi
*/
Normalize() {
const d = exports.b2_two_pi * Math.floor(this.a0 / exports.b2_two_pi);
this.a0 -= d;
this.a -= d;
}
}
exports.b2Sweep = b2Sweep;