diff --git a/src/collision/Distance.ts b/src/collision/Distance.ts
index ffbf32a3..4b02e76e 100644
--- a/src/collision/Distance.ts
+++ b/src/collision/Distance.ts
@@ -111,9 +111,6 @@ export const Distance = function (output: DistanceOutput, cache: SimplexCache, i
   const saveB = []; // int[3]
   let saveCount = 0;
 
-  let distanceSqr1 = Infinity;
-  let distanceSqr2 = Infinity;
-
   // Main iteration loop.
   let iter = 0;
   while (iter < k_maxIters) {
@@ -131,16 +128,6 @@ export const Distance = function (output: DistanceOutput, cache: SimplexCache, i
       break;
     }
 
-    // Compute closest point.
-    const p = simplex.getClosestPoint();
-    distanceSqr2 = p.lengthSquared();
-
-    // Ensure progress
-    if (distanceSqr2 >= distanceSqr1) {
-      // break;
-    }
-    distanceSqr1 = distanceSqr2;
-
     // Get search direction.
     const d = simplex.getSearchDirection();
 
diff --git a/src/collision/Shape.ts b/src/collision/Shape.ts
index b5b1c3b2..3a0cd28d 100644
--- a/src/collision/Shape.ts
+++ b/src/collision/Shape.ts
@@ -37,6 +37,11 @@ import type { Vec2Value }  from '../common/Vec2';
  */
 export abstract class Shape {
   m_type: ShapeType;
+
+  /**
+   * Radius of a shape. For polygonal shapes this must be b2_polygonRadius.
+   * There is no support for making rounded polygons.
+   */
   m_radius: number;
 
   /** @internal */
diff --git a/src/collision/shape/ChainShape.ts b/src/collision/shape/ChainShape.ts
index 9b4c12a4..c2141a84 100644
--- a/src/collision/shape/ChainShape.ts
+++ b/src/collision/shape/ChainShape.ts
@@ -148,6 +148,10 @@ export class ChainShape extends Shape {
   _createLoop(vertices: Vec2Value[]): ChainShape {
     _ASSERT && console.assert(this.m_vertices.length == 0 && this.m_count == 0);
     _ASSERT && console.assert(vertices.length >= 3);
+    if (vertices.length < 3) {
+      return;
+    }
+
     for (let i = 1; i < vertices.length; ++i) {
       const v1 = vertices[i - 1];
       const v2 = vertices[i];
diff --git a/src/collision/shape/CircleShape.ts b/src/collision/shape/CircleShape.ts
index d7b7e42e..42e4218a 100644
--- a/src/collision/shape/CircleShape.ts
+++ b/src/collision/shape/CircleShape.ts
@@ -102,11 +102,6 @@ export class CircleShape extends Shape {
     return this.m_p;
   }
 
-  getVertex(index: 0): Vec2 {
-    _ASSERT && console.assert(index == 0);
-    return this.m_p;
-  }
-
   /**
    * @internal
    * @deprecated Shapes should be treated as immutable.
diff --git a/src/collision/shape/CollideEdgePolygon.ts b/src/collision/shape/CollideEdgePolygon.ts
index b68f9ba5..ae2a8dba 100644
--- a/src/collision/shape/CollideEdgePolygon.ts
+++ b/src/collision/shape/CollideEdgePolygon.ts
@@ -291,7 +291,7 @@ export const CollideEdgePolygon = function (manifold: Manifold, edgeA: EdgeShape
     polygonBA.normals[i] = Rot.mulVec2(xf.q, polygonB.m_normals[i]);
   }
 
-  const radius = 2.0 * Settings.polygonRadius;
+  const radius = polygonB.m_radius + edgeA.m_radius;
 
   manifold.pointCount = 0;
 
diff --git a/src/collision/shape/PolygonShape.ts b/src/collision/shape/PolygonShape.ts
index 174b4ad3..24d9641f 100644
--- a/src/collision/shape/PolygonShape.ts
+++ b/src/collision/shape/PolygonShape.ts
@@ -104,11 +104,6 @@ export class PolygonShape extends Shape {
     return this.m_radius;
   }
 
-  getVertex(index: number): Vec2 {
-    _ASSERT && console.assert(0 <= index && index < this.m_count);
-    return this.m_vertices[index];
-  }
-
   /**
    * @internal
    * @deprecated Shapes should be treated as immutable.
@@ -206,6 +201,7 @@ export class PolygonShape extends Shape {
     let ih = i0;
 
     while (true) {
+      _ASSERT && console.assert(m < Settings.maxPolygonVertices);
       hull[m] = ih;
 
       let ie = 0;
diff --git a/src/dynamics/Body.ts b/src/dynamics/Body.ts
index 7c12f1bf..c0226b66 100644
--- a/src/dynamics/Body.ts
+++ b/src/dynamics/Body.ts
@@ -70,12 +70,14 @@ export interface BodyDef {
    * Linear damping is use to reduce the linear velocity. The
    * damping parameter can be larger than 1.0 but the damping effect becomes
    * sensitive to the time step when the damping parameter is large.
+   * Units are 1/time
    */
   linearDamping?: number;
   /**
    * Angular damping is use to reduce the angular velocity.
    * The damping parameter can be larger than 1.0 but the damping effect
    * becomes sensitive to the time step when the damping parameter is large.
+   * Units are 1/time
    */
   angularDamping?: number;
   /**
@@ -467,10 +469,8 @@ export class Body {
    */
   setAwake(flag: boolean): void {
     if (flag) {
-      if (this.m_awakeFlag == false) {
-        this.m_awakeFlag = true;
-        this.m_sleepTime = 0.0;
-      }
+      this.m_awakeFlag = true;
+      this.m_sleepTime = 0.0;
     } else {
       this.m_awakeFlag = false;
       this.m_sleepTime = 0.0;
diff --git a/src/dynamics/Contact.ts b/src/dynamics/Contact.ts
index a945ea43..df4842fd 100644
--- a/src/dynamics/Contact.ts
+++ b/src/dynamics/Contact.ts
@@ -39,6 +39,7 @@ import { ContactImpulse, TimeStep } from "./Solver";
 const _ASSERT = typeof ASSERT === 'undefined' ? false : ASSERT;
 
 
+// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix.
 const DEBUG_SOLVER = false;
 
 /**
@@ -85,7 +86,7 @@ export type ContactCallback = (
 
 /**
  * Friction mixing law. The idea is to allow either fixture to drive the
- * restitution to zero. For example, anything slides on ice.
+ * friction to zero. For example, anything slides on ice.
  */
 export function mixFriction(friction1: number, friction2: number): number {
   return Math.sqrt(friction1 * friction2);
@@ -910,8 +911,7 @@ export class Contact {
       // 01/07 on Box2D_Lite).
       // Build the mini LCP for this contact patch
       //
-      // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i =
-      // 1..2
+      // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
       //
       // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
       // b = vn0 - velocityBias
diff --git a/src/dynamics/Solver.ts b/src/dynamics/Solver.ts
index aaa2dc6d..67e4ac97 100644
--- a/src/dynamics/Solver.ts
+++ b/src/dynamics/Solver.ts
@@ -194,8 +194,8 @@ export class Solver {
         _ASSERT && console.assert(b.isActive() == true);
         this.addBody(b);
 
-        // Make sure the body is awake.
-        b.setAwake(true);
+        // Make sure the body is awake (without resetting sleep timer).
+        b.m_awakeFlag = true;
 
         // To keep islands as small as possible, we don't
         // propagate islands across static bodies.
diff --git a/src/dynamics/joint/MotorJoint.ts b/src/dynamics/joint/MotorJoint.ts
index fa27368d..6edbc744 100644
--- a/src/dynamics/joint/MotorJoint.ts
+++ b/src/dynamics/joint/MotorJoint.ts
@@ -135,6 +135,9 @@ export class MotorJoint extends Joint {
     // J = [-I -r1_skew I r2_skew ]
     // Identity used:
     // w k % (rx i + ry j) = w * (-ry i + rx j)
+    //
+    // r1 = offset - c1
+    // r2 = -c2
 
     // Angle constraint
     // Cdot = w2 - w1
@@ -298,11 +301,10 @@ export class MotorJoint extends Joint {
     const qB = Rot.neo(aB);
 
     // Compute the effective mass matrix.
-    this.m_rA = Rot.mulVec2(qA, Vec2.neg(this.m_localCenterA));
+    this.m_rA = Rot.mulVec2(qA, Vec2.sub(this.m_linearOffset, this.m_localCenterA));
     this.m_rB = Rot.mulVec2(qB, Vec2.neg(this.m_localCenterB));
 
     // J = [-I -r1_skew I r2_skew]
-    // [ 0 -1 0 1]
     // r_skew = [-ry; rx]
 
     // Matlab
@@ -315,6 +317,7 @@ export class MotorJoint extends Joint {
     const iA = this.m_invIA;
     const iB = this.m_invIB;
 
+    // Upper 2 by 2 of K for point to point
     const K = new Mat22();
     K.ex.x = mA + mB + iA * this.m_rA.y * this.m_rA.y + iB * this.m_rB.y * this.m_rB.y;
     K.ex.y = -iA * this.m_rA.x * this.m_rA.y - iB * this.m_rB.x * this.m_rB.y;
@@ -331,7 +334,6 @@ export class MotorJoint extends Joint {
     this.m_linearError = Vec2.zero();
     this.m_linearError.addCombine(1, cB, 1, this.m_rB);
     this.m_linearError.subCombine(1, cA, 1, this.m_rA);
-    this.m_linearError.sub(Rot.mulVec2(qA, this.m_linearOffset));
 
     this.m_angularError = aB - aA - this.m_angularOffset;
 
diff --git a/src/dynamics/joint/MouseJoint.ts b/src/dynamics/joint/MouseJoint.ts
index c9eb2a84..65be4499 100644
--- a/src/dynamics/joint/MouseJoint.ts
+++ b/src/dynamics/joint/MouseJoint.ts
@@ -194,9 +194,8 @@ export class MouseJoint extends Joint {
    * Use this to update the target point.
    */
   setTarget(target: Vec2Value): void {
-    if (this.m_bodyB.isAwake() == false) {
-      this.m_bodyB.setAwake(true);
-    }
+    if (Vec2.areEqual(target, this.m_targetA)) return;
+    this.m_bodyB.setAwake(true);
     this.m_targetA = Vec2.clone(target);
   }
 
diff --git a/src/dynamics/joint/PrismaticJoint.ts b/src/dynamics/joint/PrismaticJoint.ts
index 46e753fb..0ea323e2 100644
--- a/src/dynamics/joint/PrismaticJoint.ts
+++ b/src/dynamics/joint/PrismaticJoint.ts
@@ -449,6 +449,7 @@ export class PrismaticJoint extends Joint {
    * Enable/disable the joint motor.
    */
   enableMotor(flag: boolean): void {
+    if (flag == this.m_enableMotor) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_enableMotor = flag;
@@ -458,6 +459,7 @@ export class PrismaticJoint extends Joint {
    * Set the motor speed, usually in meters per second.
    */
   setMotorSpeed(speed: number): void {
+    if (speed == this.m_motorSpeed) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_motorSpeed = speed;
@@ -467,6 +469,7 @@ export class PrismaticJoint extends Joint {
    * Set the maximum motor force, usually in N.
    */
   setMaxMotorForce(force: number): void {
+    if (force == this.m_maxMotorForce) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_maxMotorForce = force;
diff --git a/src/dynamics/joint/RevoluteJoint.ts b/src/dynamics/joint/RevoluteJoint.ts
index b6b64ced..362ebb30 100644
--- a/src/dynamics/joint/RevoluteJoint.ts
+++ b/src/dynamics/joint/RevoluteJoint.ts
@@ -302,6 +302,7 @@ export class RevoluteJoint extends Joint {
    * Enable/disable the joint motor.
    */
   enableMotor(flag: boolean): void {
+    if (flag == this.m_enableMotor) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_enableMotor = flag;
@@ -318,6 +319,7 @@ export class RevoluteJoint extends Joint {
    * Set the motor speed in radians per second.
    */
   setMotorSpeed(speed: number): void {
+    if (speed == this.m_motorSpeed) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_motorSpeed = speed;
@@ -334,6 +336,7 @@ export class RevoluteJoint extends Joint {
    * Set the maximum motor torque, usually in N-m.
    */
   setMaxMotorTorque(torque: number): void {
+    if (torque == this.m_maxMotorTorque) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_maxMotorTorque = torque;
diff --git a/src/dynamics/joint/WheelJoint.ts b/src/dynamics/joint/WheelJoint.ts
index 1c527c70..8c9bae74 100644
--- a/src/dynamics/joint/WheelJoint.ts
+++ b/src/dynamics/joint/WheelJoint.ts
@@ -314,6 +314,7 @@ export class WheelJoint extends Joint {
    * Enable/disable the joint motor.
    */
   enableMotor(flag: boolean): void {
+    if (flag == this.m_enableMotor) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_enableMotor = flag;
@@ -323,6 +324,7 @@ export class WheelJoint extends Joint {
    * Set the motor speed, usually in radians per second.
    */
   setMotorSpeed(speed: number): void {
+    if (speed == this.m_motorSpeed) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_motorSpeed = speed;
@@ -339,6 +341,7 @@ export class WheelJoint extends Joint {
    * Set/Get the maximum motor force, usually in N-m.
    */
   setMaxMotorTorque(torque: number): void {
+    if (torque == this.m_maxMotorTorque) return;
     this.m_bodyA.setAwake(true);
     this.m_bodyB.setAwake(true);
     this.m_maxMotorTorque = torque;