@dimension-mismatch/2dphysics 0.0.1

package/body.ts

@@ -0,0 +1,252 @@
+import { Rotation, vec2 } from "../vec2/calc.js";
+import { Circle, Polygon, Shape, ShapeType } from "./geometry.js";
+let uniqueID: number = 0;
+
+export class Material{
+    bounciness: number; 
+    friction: number;
+    staticFriction: number;
+    density: number;
+
+    constructor(bounciness: number, friction: number, staticFriction: number, density: number){
+        this.bounciness = bounciness;
+        this.friction = friction;
+        this.staticFriction = staticFriction;
+        this.density = density;
+    }
+    static default(){
+        return new Material(0.2, 0.3, 0.4, 1);
+    } 
+}
+export interface GameObject{
+    uniqueID: number;
+    isCollection: boolean;
+    getAllObjects(): any;
+}
+export class Collection implements GameObject{
+    isCollection = true;
+    children: GameObject[] = [];
+    uniqueID: number;
+
+    constructor(children: GameObject[]){
+        this.uniqueID = uniqueID++;
+        this.children = children;
+    }
+    static ofObjects(...objects: GameObject[]){
+        return new Collection(objects)
+    }
+    addObjects(...objects: GameObject[]){
+        this.children.push(...objects);
+        return this;
+    }
+    getAllObjects(): PhysicsObject[]{
+        let objects = [];
+        for(let i = 0; i < this.children.length; i++){
+            if( this.children[i].isCollection){
+                objects.push(this.children[i].getAllObjects())
+            }
+            else{
+                objects.push(this.children[i]);
+            }
+        }
+        return objects;
+    }
+}
+export interface PhysicsObjectOptions{
+    angle?: Rotation;
+    velocity?: vec2;
+    angularVelocity?: Rotation;
+    bounciness?: number;
+    staticFriction?: number;
+    friction?: number;
+    density?: number; 
+    mass?: number;
+    material?: Material;
+    static?: boolean;
+    skipCOMcalc?: boolean;
+
+}
+export class PhysicsObject implements GameObject{
+    getAllObjects() {
+        return null;
+    }
+    isCollection = false;
+    position: vec2;
+    angle: Rotation;
+    uniqueID: number;
+    velocity: vec2;
+    angularVelocity: Rotation;
+
+    lastPosition: vec2;
+    lastAngle: Rotation;
+
+    deltaTime: number = 1;
+
+    acceleration: vec2 = vec2.zero();
+    angularAccerleration: Rotation = Rotation.zero();
+
+    mass: number;
+    inverseMass: number;
+
+    inertia: number;
+    inverseInertia: number;
+
+
+    material: Material;
+
+    colliders: Shape[];
+    constructor(position: vec2, colliders: Shape[] | Shape, options?: PhysicsObjectOptions){
+        this.uniqueID = uniqueID++;
+        this.position  = position;
+        if(options && options.angle){
+            this.angle = options.angle;
+        }
+        else{
+            this.angle = Rotation.zero();
+        }
+        
+
+        this.lastPosition = position.copy();
+        this.lastAngle = this.angle.copy();
+
+        this.velocity = vec2.zero();
+        this.angularVelocity = Rotation.zero();
+
+        if(Array.isArray(colliders)){
+            this.colliders = colliders;
+        }
+        else{
+            this.colliders = [colliders];
+        }
+        
+        if(options){
+            if(options.velocity){
+                this.lastPosition.subtract(options.velocity);
+                this.velocity = options.velocity.copy();
+            }
+            if(options.angularVelocity){
+                this.lastAngle.subtract(options.angularVelocity);
+                this.angularVelocity = options.angularVelocity.copy();
+            }
+            if(options.material){
+                this.material = options.material;
+            }
+            else{
+                this.material = Material.default();
+            }
+            if(options.bounciness){
+                this.material.bounciness = options.bounciness;
+            }
+            if(options.density){
+                this.material.density = options.density;
+            }
+            if(options.staticFriction){
+                this.material.staticFriction = options.staticFriction;
+            }
+            if(options.friction){
+                this.material.friction = options.friction;
+            }
+        }
+        else{
+            this.material = Material.default();
+        }
+        this.calculateProperties(options && options.skipCOMcalc);
+        if(options && options.static){
+            this.inverseMass = 0;
+            this.inverseInertia = 0;
+        }
+        if(options?.mass){
+            let factor = options.mass / this.mass;
+            this.mass *= factor;
+            this.inertia *= factor;
+            this.inverseMass /= factor;
+            this.inverseInertia /= factor;
+            console.log(this);
+        }
+    }
+    private calculateProperties(skipCOM?: boolean){
+        let COM = new vec2(0,0);
+        let totalArea = 0;
+        for(let i = 0; i < this.colliders.length; i++){
+            totalArea += this.colliders[i].area;
+            COM.add(vec2.times(this.colliders[i].COM, this.colliders[i].area));
+        }
+        COM.divideBy(totalArea);
+        this.mass = totalArea * this.material.density;
+        if(skipCOM){
+            COM = new vec2(0,0);
+        }
+        let inertia = 0;
+        for(let i = 0; i < this.colliders.length; i++){
+            this.colliders[i].translate(COM.inverse());
+            inertia += this.colliders[i].inertia;
+        }
+        this.inertia = inertia * this.material.density;
+
+        this.inverseMass = 1/this.mass;
+        this.inverseInertia = 1/this.inertia;
+    }
+    worldToLocalSpace(v: vec2): vec2{
+        return vec2.worldToLocalSpace(v, this.position, this.angle);
+    }
+    localToWorldSpace(v: vec2): vec2{
+        return vec2.localToWorldSpace(v, this.position, this.angle);
+    }
+    localToAASpace(v: vec2): vec2{
+        return vec2.localToWorldSpace(v, vec2.zero(), this.angle);
+    }
+    applyForce(force: vec2, location: vec2){
+        this.acceleration.add(vec2.times(force, this.inverseMass));
+        this.angularAccerleration.add(Rotation.new(vec2.cross(location, force) * this.inverseInertia));
+    }
+    translate(t: vec2, updateVelocity: boolean){
+        this.position.add(t);
+        if(!updateVelocity){
+            this.lastPosition.add(t);
+        }
+    }
+    setPosition(p: vec2, updateVelocity: boolean){
+        if(!updateVelocity){
+            this.lastPosition.add(vec2.minus(p, this.position));
+            this.velocity.add(vec2.minus(p, this.position));
+        }
+        this.position = p;
+    }
+    setVelocity(v: vec2){
+        this.lastPosition = vec2.minus(this.position, v);
+        this.velocity = v.copy();
+    }
+    rotate(r: Rotation, updateVelocity: boolean){
+        this.angle.add(r);
+        if(!updateVelocity){
+            this.lastAngle.add(r);
+        }
+    }
+    getVelocity(): vec2{
+        return this.velocity.copy();
+        return vec2.minus(this.position, this.lastPosition).divideBy(this.deltaTime);
+    }
+    getAngularVelocity(): Rotation{
+        return this.angularVelocity.copy();
+        return Rotation.times(Rotation.minus(this.angle, this.lastAngle),1 / this.deltaTime);
+    }
+
+    getVelocityOfPoint(point: vec2): vec2{
+        return vec2.plus(this.getVelocity(), vec2.times(vec2.rotatedBy(point, Rotation.ccw90deg()), this.getAngularVelocity().angle));
+    }
+
+    getVelocityOfLocalPoint(point: vec2): vec2{
+        return vec2.plus(this.getVelocity(), vec2.times(vec2.rotatedBy(point, Rotation.plus(this.angle, Rotation.ccw90deg())), this.getAngularVelocity().angle));
+    }
+
+    //static readonly empty = new PhysicsObject(vec2.zero, Rotation.zero, [], {density: Infinity});
+    static rectangle(position: vec2, width: number, height: number, options?: PhysicsObjectOptions): PhysicsObject{
+        return new PhysicsObject(position, [Polygon.rectangle(vec2.zero(), width, height)], options);
+    }
+    static regularPolygon(position: vec2, radius: number, sides: number, options?: PhysicsObjectOptions): PhysicsObject{
+        return new PhysicsObject(position, [Polygon.regularPolygon(vec2.zero(), radius, sides)], options);
+    }
+    static circle(position: vec2, radius: number, options?: PhysicsObjectOptions): PhysicsObject{
+        return new PhysicsObject(position, [new Circle(vec2.zero(), radius)], options);
+    }
+}

package/collision.ts

@@ -0,0 +1,284 @@
+import { PhysicsObject } from "./body.js";
+import { Rotation, vec2 } from "../vec2/calc.js";
+import { Circle, Polygon, Shape, ShapeType, Vertex } from "./geometry.js";
+
+export interface Contact{
+    depth: number;
+    normal: vec2;
+    objectA: PhysicsObject;
+    objectB: PhysicsObject;
+
+    shapeA: Shape;
+    shapeB: Shape;
+
+    contactPoints: vec2[];
+}
+
+export interface SATresult{
+    axis: vec2;
+    depth: number;
+    Aindex: number;
+    Bindex: number;
+}
+function invert(r: Contact){
+    r.normal = r.normal.inverse();
+
+    let objectB = r.objectB;
+    r.objectB = r.objectA;
+    r.objectA = objectB;
+
+    let shapeB = r.shapeB;
+    r.shapeB = r.shapeA;
+    r.shapeA = shapeB;
+}
+function objectToVertexSpace(v: vec2, vert: Vertex): vec2{
+    return vec2.worldToLocalSpace(v , vert.position, new Rotation(0, vert.normal.x, vert.normal.y))
+}
+function worldToVertexSpace(v: vec2, objectA: PhysicsObject, objectB: PhysicsObject, vert: Vertex): vec2{
+    return objectToVertexSpace(objectB.worldToLocalSpace(objectA.localToWorldSpace(v)), vert);
+}
+function vertexToWorldSpace(v: vec2, objectB: PhysicsObject, vert: Vertex){
+    return objectB.localToWorldSpace(vec2.localToWorldSpace(v , vert.position, {angle: 0, cos: vert.normal.x, sin: vert.normal.y} as Rotation));
+}
+function SAT(shapeA: Polygon, shapeB: Polygon, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    let bestResult: SATresult = {axis: new vec2(0,0), depth: Infinity, Aindex: 0, Bindex: 0}
+
+    //find which normal of shapeB has the least overlap
+    for(let axis = 0; axis < shapeB.vertices.length; axis++){
+        
+        let AminProjection = Infinity;
+        let mindex: number;
+
+        let normal = shapeB.vertices[axis].normal;
+        let BmaxProjection = vec2.dot(shapeB.vertices[axis].position, normal);
+
+        for(let vertex = 0; vertex < shapeA.vertices.length; vertex++){
+            let worldPosition = objectA.localToWorldSpace(shapeA.vertices[vertex].position);
+            let localPosition = objectB.worldToLocalSpace(worldPosition);
+
+            let proj = vec2.dot(localPosition, normal);
+            if(proj < AminProjection){
+                AminProjection = proj;
+                mindex = vertex;
+            }
+
+        }
+        if(AminProjection > BmaxProjection){         
+            return false;
+        }
+        if(shapeB.vertices[axis].isInternal){
+            continue;
+        }
+        if(BmaxProjection - AminProjection < bestResult.depth){
+            bestResult.depth = BmaxProjection - AminProjection;
+            bestResult.axis = normal;
+            bestResult.Bindex = axis;
+            bestResult.Aindex = mindex;
+        }
+    }
+    //find which face of shapeA is intersecting shapeB
+    let n1idx = bestResult.Aindex; 
+    let n1 = shapeA.vertices[n1idx].normal;
+
+    let n2idx = (n1idx + 1) % shapeA.vertices.length;
+    let n0idx = (n1idx - 1) % shapeA.vertices.length;
+
+    let b1idx = bestResult.Bindex == 0? shapeB.vertices.length - 1 :bestResult.Bindex - 1;
+
+    
+    let n2 = shapeA.vertices[n1idx].normal;
+
+    
+    n1 = vec2.rotatedBy(n1, objectA.angle).rotateBy(Rotation.inverse(objectB.angle));
+    n2 = vec2.rotatedBy(n2, objectA.angle).rotateBy(Rotation.inverse(objectB.angle));
+
+    let contactPoints: vec2[];
+    if(vec2.dot(n1, bestResult.axis) < vec2.dot(n2, bestResult.axis)){
+        contactPoints = [shapeA.vertices[n0idx].position, shapeA.vertices[n1idx].position];
+    }
+    else{
+        contactPoints = [shapeA.vertices[n1idx].position, shapeA.vertices[n2idx].position];
+    }
+    contactPoints = contactPoints.map((v) => (worldToVertexSpace(v, objectA, objectB, shapeB.vertices[bestResult.Bindex])));
+    let b2 = objectToVertexSpace(shapeB.vertices[b1idx].position, shapeB.vertices[bestResult.Bindex]);
+
+    for(let i = contactPoints.length - 1; i >= 0; i--){
+        if(contactPoints[i].x > 0){
+            contactPoints.splice(i);
+            continue;
+        }
+        else{
+            contactPoints[i].x = 0;
+        }
+        if(contactPoints[i].y > 0){
+            contactPoints[i].y = 0;
+        }
+        if(contactPoints[i].y < b2.y){
+            contactPoints[i].y = b2.y;
+        }
+    }
+    contactPoints = contactPoints.map((v) => (vertexToWorldSpace(v, objectB, shapeB.vertices[bestResult.Bindex])));
+    return {
+        shapeA: shapeA,
+        shapeB: shapeB,
+
+        objectA: objectA,
+        objectB: objectB,
+
+        normal: vec2.rotatedBy(bestResult.axis,objectB.angle),
+        depth: bestResult.depth,
+
+        contactPoints: contactPoints
+
+    }
+    //return bestResult;
+}
+export function PolygonCollsion(shapeA: Polygon, shapeB: Polygon, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    let rA = SAT(shapeA, shapeB, objectA, objectB);
+    if(!rA){
+        return false;
+    }
+    let rB = SAT(shapeB, shapeA, objectB, objectA);
+    if(!rB){
+        return false;
+    }
+    if(rB.depth < rA.depth){
+        invert(rB);
+        return rB;
+    }
+    else{
+        return rA;
+    }
+}
+export function CirclePolygonCollision(shapeA: Circle, shapeB: Polygon, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    let localCenter = objectB.worldToLocalSpace(objectA.localToWorldSpace(shapeA.COM));
+
+    let bestResult: {distance: number, normal: vec2, contact: vec2};
+    bestResult = {distance: Infinity, normal: vec2.zero(), contact: vec2.zero()};
+    
+    let lastPoint = shapeB.vertices[shapeB.vertices.length - 1].position;
+    for(let i = 0; i < shapeB.vertices.length; i++){
+        let vertex = shapeB.vertices[i];
+        
+        let relativeCenter = objectToVertexSpace(localCenter, vertex);
+        let relativeLast = objectToVertexSpace(lastPoint, vertex);
+        lastPoint = vertex.position;
+        
+        
+        let currentResult: {distance: number, normal: vec2, contact: vec2};
+       
+
+        if(relativeCenter.y > 0){
+            currentResult = {
+                distance: relativeCenter.mag(),
+                contact: new vec2(0,0),
+                normal: vec2.asUnitVector(vec2.minus(localCenter, vertex.position))}
+                     
+        }
+        else{
+            if(relativeCenter.x > shapeA.radius){
+                return false;
+            }
+            if(relativeCenter.y < relativeLast.y || vertex.isInternal){
+                continue;
+            }
+            currentResult = {
+                distance: relativeCenter.x,
+                contact: new vec2(0, relativeCenter.y),
+                normal: vertex.normal}
+        }
+
+        
+        
+        
+
+        
+        if(Math.abs(currentResult.distance) < Math.abs(bestResult.distance)){
+            bestResult.distance = currentResult.distance;
+            bestResult.normal = currentResult.normal;
+            bestResult.contact = vertexToWorldSpace(currentResult.contact, objectB, vertex);
+        }
+    }
+    if(bestResult.distance > shapeA.radius){
+        return false;
+    }
+    return {
+        shapeA: shapeA,
+        shapeB: shapeB,
+
+        objectA: objectA,
+        objectB: objectB,
+
+        normal: vec2.rotatedBy(bestResult.normal, objectB.angle),
+        depth: shapeA.radius - bestResult.distance,
+
+        contactPoints: [bestResult.contact]
+    }
+}
+export function PolygonCircleCollsion(shapeA: Polygon, shapeB: Circle, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    let ret = CirclePolygonCollision(shapeB, shapeA, objectB, objectA);
+    if(!ret){
+        return false;
+    }
+    else{
+        invert(ret);
+        return ret;
+    }
+}
+
+export function CircleCircleCollision(shapeA: Circle, shapeB: Circle, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    let Acenter = objectB.worldToLocalSpace(objectA.localToWorldSpace(shapeA.COM));
+    let between = vec2.minus(Acenter, shapeB.COM);
+    let dist = between.mag();
+    if(between.x == 0 && between.y == 0){
+        between.y = 1;
+    }
+    if(dist > shapeA.radius + shapeB.radius){
+        return false;
+    }
+    else{
+        let normal = vec2.dividedBy(between, dist);
+        return{
+            depth: shapeA.radius + shapeB.radius - dist,
+            normal: normal.rotateBy(objectB.angle),
+
+            shapeA: shapeA,
+            shapeB: shapeB,
+
+            objectA: objectA,
+            objectB: objectB,
+
+            contactPoints: [objectB.localToWorldSpace(vec2.times(normal, shapeB.radius))]
+        }
+    }
+}
+export function ShapeCollision(shapeA: Shape, shapeB: Shape, objectA: PhysicsObject, objectB: PhysicsObject): Contact | false{
+    if(shapeA.type == ShapeType.CIRCLE){
+        if(shapeB.type == ShapeType.CIRCLE){
+            return CircleCircleCollision(shapeA as Circle, shapeB as Circle, objectA, objectB);
+        }
+        else{
+            return CirclePolygonCollision(shapeA as Circle, shapeB as Polygon, objectA, objectB);
+        }
+    }
+    else{
+        if(shapeB.type == ShapeType.CIRCLE){
+            return PolygonCircleCollsion(shapeA as Polygon, shapeB as Circle, objectA, objectB);
+        }
+        else{
+            return PolygonCollsion(shapeA as Polygon, shapeB as Polygon, objectA, objectB);
+        }
+    }
+}
+export function Collision(objectA: PhysicsObject, objectB: PhysicsObject): Contact[]{
+    let results: Contact[] = [];
+    for(let i = 0; i < objectA.colliders.length; i++){
+        for(let j = 0; j < objectB.colliders.length; j++){
+            let res = ShapeCollision(objectA.colliders[i], objectB.colliders[j], objectA, objectB);
+            if(res){
+                results.push(res);
+            }
+        }
+    }
+    return results;
+}

package/constraints/constraint.ts

@@ -0,0 +1,17 @@
+import { PhysicsObject } from "../body.js";
+import { Rotation, vec2 } from "../../vec2/calc.js";
+export enum ConstraintType{
+  Spring,
+  Mouse,
+  Wheel,
+  Motor,
+}
+export interface Constraint{
+  solveVelocity(dt: number): void;
+  solvePosition(dt: number): void;
+  constraintType: ConstraintType;
+}
+
+
+
+

package/constraints/springs.ts

@@ -0,0 +1,93 @@
+import { PhysicsObject } from "../body.js";
+import { vec2 } from "../../vec2/calc.js";
+import { Constraint, ConstraintType } from "./constraint.js";
+export interface SpringOptions{
+  pointA?: vec2;
+  pointB?: vec2;
+  k?: number;
+  damping?: number;
+  length?: number;
+}
+export class Spring implements Constraint{
+  constraintType = ConstraintType.Spring;
+  objectA: PhysicsObject;
+  objectB: PhysicsObject | null;
+  pointA: vec2;
+  pointB: vec2;
+  k: number;
+  damping: number;
+  length: number;
+  constructor(objectA: PhysicsObject, objectB: PhysicsObject | null, options?: SpringOptions){
+    this.objectA = objectA;
+    this.objectB = objectB;
+    this.pointA = options?.pointA? options.pointA : vec2.zero();
+    this.pointB = options?.pointB? options.pointB : vec2.zero();
+    this.k = options?.k? options.k : 1;
+    this.damping = options?.damping? options.damping: 0;
+    this.length = options?.length? options.length: 0;
+  }
+  getPointA(){
+    return this.objectA.localToWorldSpace(this.pointA);
+  }
+  getPointB(){
+    return this.objectB? this.objectB.localToWorldSpace(this.pointB) : this.pointB;
+  }
+  getVelA(){
+    return this.objectA.getVelocityOfLocalPoint(this.pointA);
+  }
+  getVelB(){
+    return this.objectB? this.objectB.getVelocityOfLocalPoint(this.pointB): vec2.zero();
+  }
+  solveVelocity(dt: number): void {
+    let displacement = vec2.minus(this.getPointB(), this.getPointA());
+    let direction = displacement.normalize();
+
+
+    let relativeVelocity = vec2.minus(this.getVelB(), this.getVelA());
+    let normalVelocity = vec2.dot(direction, relativeVelocity);
+    //F + gx' + kx. 0
+    //hooke's law: F = -kx
+    let force = this.k * (displacement.mag() - this.length);
+    let v = vec2.plus(vec2.times(direction, force * dt), vec2.times(relativeVelocity, this.damping * dt));
+    this.objectA.applyForce(v, this.objectA.localToAASpace(this.pointA));
+    if(this.objectB){
+      this.objectB.applyForce(vec2.times(v, -1), this.objectB.localToAASpace(this.pointB));
+    }
+  }
+  solvePosition(): void {
+      //springs only apply forces
+  }
+}
+ 
+export class MouseConstraint implements Constraint{
+  constraintType = ConstraintType.Mouse;
+  spring: Spring;
+  enabled: boolean
+  constructor(){
+    this.enabled = false;
+  }
+  solveVelocity(dt: number): void {
+    if(this.enabled){
+      this.spring.solveVelocity(dt);
+    }
+  }
+  solvePosition(dt: number): void {
+    if(this.enabled){
+      this.spring.solvePosition();
+    }
+  }
+  enable(object: PhysicsObject, location: vec2, mousePosition: vec2){
+    this.spring = new Spring(object,null, {pointA: location, pointB: mousePosition, k: 100 * object.mass, damping: 8 * object.mass});
+    this.enabled = true;
+  }
+  disable(){
+    this.spring = null;
+    this.enabled = false;
+  }
+  update(mousePosition: vec2){
+    if(this.enabled){
+      this.spring.pointB = mousePosition;
+    }
+    
+  }
+}

package/constraints/wheel.ts

@@ -0,0 +1,191 @@
+import { PhysicsObject } from "../body.js";
+import { vec2, Rotation } from "../../vec2/calc.js";
+import { Constraint, ConstraintType } from "./constraint.js";
+export interface MotorDrivable extends Constraint{
+  applyMotorTorque(torque: number): void;
+  getAngularVelocity(): number;
+}
+export interface WheelOptions{
+  cof?: number;
+  static_cof?: number;
+  density?: number;
+  radius?: number;
+  wheelCount?: number;
+  angle?: Rotation;
+
+}
+export class Wheel implements MotorDrivable{
+  constraintType = ConstraintType.Wheel;
+  wheelCount: number = 1;
+  appliedTorque: number = 0;
+  frictionForce: vec2 = new vec2(0,0);
+  constructor(mountedTo: PhysicsObject, position: vec2, options?: WheelOptions){
+    this.mountedTo = mountedTo;
+    this.position = position.copy();
+    this.cof = 1.0;
+    this.static_cof = 1.2;
+    this.angularVelocity = 0;
+    this.radius = 1;
+    this.angle = Rotation.zero();
+    this.wheelCount = options?.wheelCount ? options.wheelCount : 1;
+    
+    let density = 100;
+    if(options){
+      if(options.angle){
+        this.angle = options.angle.copy();
+      }
+      if(options.cof){
+        this.cof = options.cof;
+      }
+      if(options.density){
+        density = options.density;
+      }
+      if(options.radius){
+        this.radius = options.radius;
+      }
+      if(options.static_cof){
+        this.static_cof = options.static_cof;
+      }
+      
+    }
+    let area = Math.PI * this.radius * this.radius;
+    this.momentOfInertia = area * this.radius * this.radius * density;
+  }
+  applyMotorTorque(torque: number): void {
+    this.appliedTorque = torque;
+  }
+  getAngularVelocity(): number {
+    return this.angularVelocity;
+  }
+  solveVelocity(dt: number): void {
+    let axis = vec2.rotatedBy(this.angle.unitVector(), this.mountedTo.angle);
+    this.angularVelocity += this.appliedTorque / this.momentOfInertia * dt;
+
+    let relativeVelocity: vec2 = this.mountedTo.getVelocityOfLocalPoint(this.position);
+    let surfaceVelocity: vec2 = vec2.times(axis, this.angularVelocity * this.radius);
+    relativeVelocity.add(surfaceVelocity);
+
+    let AxN = vec2.cross(this.mountedTo.localToAASpace(this.position), vec2.asUnitVector(relativeVelocity));
+    let massFactor = this.mountedTo.inverseMass + this.mountedTo.inverseInertia * AxN * AxN;
+
+
+    let normalForce: number = 9.8 / massFactor / this.wheelCount;
+    
+    let antiAlignedVelocity = vec2.cross( axis, relativeVelocity);
+    
+    let stoppingForce = antiAlignedVelocity / massFactor / this.wheelCount;
+
+    let slidingForce = -Math.sign(antiAlignedVelocity) * normalForce * dt;
+
+    let antiAlignedForce = Math.abs(stoppingForce) < normalForce * this.static_cof? stoppingForce : slidingForce;
+   
+    this.frictionForce = vec2.times(vec2.tangent(axis), antiAlignedForce);
+    
+    this.mountedTo.applyForce(this.frictionForce, this.mountedTo.localToAASpace(this.position));
+    
+    let alignedVelocity = vec2.dot(axis, relativeVelocity);
+    //1/2 I w_0^2 = 1/2mv^2 + 1/2 Iw^2, v = rw
+    //w = w_0 * sqrt(I / (I + mr^2))
+    let eqVelocity = this.radius * this.angularVelocity * Math.sqrt(this.momentOfInertia / (this.momentOfInertia + this.mountedTo.mass * this.radius * this.radius));
+
+    stoppingForce = (eqVelocity - alignedVelocity) / massFactor / this.wheelCount;
+    if(Math.abs(stoppingForce) < normalForce * this.static_cof){
+      let stoppingTorque = (eqVelocity - alignedVelocity) / this.radius * this.momentOfInertia / this.wheelCount;
+
+      this.mountedTo.applyForce(vec2.times(axis, stoppingForce), this.mountedTo.localToAASpace(this.position));
+      this.angularVelocity += stoppingTorque / this.momentOfInertia;
+    }
+    else{
+      slidingForce = -Math.sign(alignedVelocity) * normalForce * dt;
+      this.mountedTo.applyForce(vec2.times(axis, slidingForce), this.mountedTo.localToAASpace(this.position));
+      this.angularVelocity += slidingForce / this.momentOfInertia * this.radius;
+    }
+    
+   
+  }
+  solvePosition(dt: number): void {
+
+  }
+  cof: number;
+  static_cof: number;
+
+  
+  angularVelocity: number;
+  radius: number;
+
+  momentOfInertia: number;
+
+
+  
+  mountedTo: PhysicsObject;
+  position: vec2;
+  angle: Rotation;
+  
+}
+
+export interface DCMotorOptions{
+  nominalVoltage?: number;
+  stallTorque?: number;
+  stallCurrent?: number;
+  freeCurrent?: number;
+  freeSpeed?: number;
+  gearRatio?: number;
+}
+
+export class DCMotor implements Constraint{
+  nominalVoltage: number;
+  stallTorque: number;
+  stallCurrent: number;
+  freeCurrent: number;
+  freeSpeed: number;
+  gearRatio: number;
+
+
+  internalResistance: number;
+  Kt: number; // N*m/A
+  Kv: number; // Rad/s/V
+
+
+  output: MotorDrivable;
+
+
+  inputVoltage: number = 0;
+  filteredVoltage: number = 0;
+  constructor(output: MotorDrivable, options?: DCMotorOptions){
+    //default values from KrakenX60
+    this.nominalVoltage = options?.nominalVoltage? options.nominalVoltage : 12;
+    this.stallTorque = options?.stallTorque? options.stallTorque : 7.09;
+    this.stallCurrent = options?.stallCurrent? options.stallCurrent : 366;
+    this.freeCurrent = options?.freeCurrent? options.freeCurrent : 2;
+    this.freeSpeed = options?.freeSpeed? options.freeSpeed : 6000 / 60 * 2 * Math.PI;
+
+    this.gearRatio = options?.gearRatio? options.gearRatio : 1;
+    this.output = output;
+
+    this.stallTorque *= this.gearRatio;
+    this.freeSpeed /= this.gearRatio;
+
+    this.internalResistance = this.nominalVoltage / this.stallCurrent; //Ohm's law: R = V/I
+    this.Kt = this.stallTorque / this.stallCurrent;
+    this.Kv = this.freeSpeed / (this.nominalVoltage - this.internalResistance * this.freeCurrent); //Find slope of the speed-voltage line
+  }
+  setInputVoltage(volts: number){
+    this.inputVoltage = volts;
+  }
+  solveVelocity(dt: number): void {
+    this.filteredVoltage = this.inputVoltage;
+    let angularVelocity = this.output.getAngularVelocity();
+    //input voltage minus back emf voltage?
+    let V = this.filteredVoltage - angularVelocity / this.Kv;
+    let I = V / this.internalResistance;
+    let T = I * this.Kt;
+
+    
+    this.output.applyMotorTorque(T);
+    this.output.solveVelocity(dt);
+  }
+  solvePosition(dt: number): void {
+    this.output.solvePosition(dt);
+  }
+  constraintType = ConstraintType.Motor;
+}

package/geometry.ts

@@ -0,0 +1,98 @@
+import { Rotation, vec2 } from "../vec2/calc.js";
+
+export class Vertex{
+    position: vec2;
+    normal: vec2;
+    isInternal: boolean;
+}
+export enum ShapeType{
+    POLYGON,
+    CIRCLE
+}
+export type Shape = Polygon | Circle;
+
+
+export class Polygon{
+    type = ShapeType.POLYGON;
+    area: number;
+    inertia: number;
+    COM: vec2;
+    vertices: Vertex[];
+    constructor(points: vec2[], internalEdges?: boolean[]){
+        let totalArea = 0;
+        this.COM = vec2.zero();
+        let lastPoint = points[points.length - 1];
+        this.vertices = [];
+        this.inertia = 0;
+        for(let i = 0; i < points.length; i++){
+            let triArea = vec2.cross(lastPoint, points[i]) / 2;
+            let triCom = vec2.times(vec2.plus(lastPoint, points[i]), triArea);
+            this.inertia += (lastPoint.magSqr() + vec2.dot(lastPoint, points[i]) + points[i].magSqr()) * triArea;
+
+            totalArea += triArea;
+            this.COM.add(triCom);
+
+            this.vertices.push(
+                {position: points[i], 
+                 normal: vec2.minus(points[i], lastPoint).normalize().rotateBy(Rotation.cw90deg()), 
+                 isInternal: internalEdges? internalEdges[i]: false});
+            lastPoint = points[i];
+        }
+        this.COM.divideBy(totalArea * 3);
+        this.inertia /= 6;
+        this.area = totalArea;
+    }
+    translate(t: vec2){
+        for(let i = 0; i < this.vertices.length; i++){
+            this.vertices[i].position.add(t);
+        }
+        this.inertia -= this.COM.magSqr() * this.area;
+        this.COM.add(t);
+        this.inertia += this.COM.magSqr() * this.area;
+
+    }
+    static rectangle(position: vec2, width: number , height: number){
+        let x = position.x;
+        let y = position.y;
+        let hw = width / 2;
+        let hh = height / 2;
+        return new Polygon(
+            [new vec2(x - hw, y - hh),
+             new vec2(x + hw, y - hh),
+             new vec2(x + hw, y + hh),
+             new vec2(x - hw, y + hh)]);
+    }
+    static cornerRect(c1: vec2, c2: vec2){
+        return Polygon.rectangle(vec2.plus(c1,c2).divideBy(2), Math.abs(c2.x - c1.x), Math.abs(c2.y - c1.y));
+    }
+    static regularPolygon(position: vec2, radius: number, sides: number){
+        let points = [];
+        let rot = Rotation.new(2 * Math.PI / sides)
+        let vertex = vec2.rotatedBy(new vec2(0, -radius), Rotation.times(rot, 0.5));
+        for(let i = 0; i < sides; i++){
+            points.push(vec2.plus(vertex, position));
+            vertex.rotateBy(rot);
+        }
+        return new Polygon(points);
+    }
+}
+export class Circle{
+    type = ShapeType.CIRCLE;
+    area: number;
+    inertia: number;
+    COM: vec2;
+    radius: number;
+    
+    constructor(position: vec2 , radius: number){
+        this.area = Math.PI * radius * radius;
+        this.inertia = this.area * radius * radius + 0.5 * position.magSqr() * this.area;
+        this.COM = position;
+        this.radius = radius;
+    }
+    translate(t: vec2){
+        this.inertia -= this.COM.magSqr() * this.area;
+        this.COM.add(t);
+        this.inertia += this.COM.magSqr() * this.area;
+
+    }
+}

package/package.json

@@ -0,0 +1,11 @@
+{
+  "name": "@dimension-mismatch/2dphysics",
+  "version": "0.0.1",
+  "description": "A simple 2d rigid-body physics engine",
+  "main": "svgtools.js",
+  "author": "dimension-mismatch",
+  "license": "ISC",
+  "dependencies": {
+    "@dimension-mismatch/vec2": "^0.0.1"
+  }
+}

package/solver.ts

@@ -0,0 +1,95 @@
+import { Rotation, vec2 } from "../vec2/calc.js";
+import { Contact } from "./collision.js";
+
+interface massFactor{
+    a: number;
+    b: number;  
+}
+function getMassFactor(am: number, bm: number): massFactor{
+    let sum = 1 / (am + bm);
+    return {a: am * sum, b: bm * sum}
+}
+export class Solver{
+    positionIterations: number;
+    velocityIterations: number = 1;
+    positionCorrectionPercent: number = 0.6;
+
+    resolvePositions(contacts: Contact[]){
+        contacts.map((c) => this.resolvePosition(c));
+    }
+    resolveVelocities(contacts: Contact[]){
+
+        contacts.map((c) => this.applyImpulses(c));
+    }
+    
+    resolvePosition(c: Contact){
+       let factor = getMassFactor(c.objectA.inverseMass, c.objectB.inverseMass);
+
+       c.objectA.translate(vec2.times(c.normal, factor.a * c.depth * this.positionCorrectionPercent), false);
+       c.objectB.translate(vec2.times(c.normal, -1 * factor.b * c.depth * this.positionCorrectionPercent), false);
+    }
+
+    applyImpulses(c: Contact){
+        let objectA = c.objectA;
+        let objectB = c.objectB;
+
+        let normal = c.normal;
+        let tangent = vec2.rotatedBy(normal, Rotation.ccw90deg());
+        let depth = c.depth;
+
+        let nContacts = c.contactPoints.length;
+
+        // let iFactor: massFactor;
+        // let avgContact = vec2.zero();
+        // for(let i = 0; i < nContacts; i++){
+            
+        //     avgContact.add(c.contactPoints[i]);
+        // }
+        // avgContact.divideBy(nContacts);
+        // let avgA = vec2.minus(avgContact, objectA.position);
+        // let avgB = vec2.minus(avgContact, objectB.position);
+
+
+        for(let i = 0; i < nContacts; i++){
+            let contactA = vec2.minus(c.contactPoints[i], objectA.position);
+            let contactB = vec2.minus(c.contactPoints[i], objectB.position);
+
+            let relativeVelocity = vec2.minus(objectA.getVelocityOfPoint(contactA), objectB.getVelocityOfPoint(contactB));
+
+            let dot = vec2.dot(relativeVelocity, normal);
+            if(dot > 0){
+                continue;
+            }
+            
+            let restitution = Math.min(objectA.material.bounciness, objectB.material.bounciness);
+            let AxN = vec2.cross(contactA, normal);
+            let BxN = vec2.cross(contactB, normal);
+            let massFactors = nContacts * (objectA.inverseMass + objectB.inverseMass + objectA.inverseInertia * AxN * AxN + objectB.inverseInertia * BxN * BxN);
+            let impulseMagnitude = -(restitution + 1) * dot / massFactors;
+
+            objectA.applyForce(vec2.times(normal, impulseMagnitude), contactA); 
+            objectB.applyForce(vec2.times(normal, -impulseMagnitude), contactB);
+
+            let cof = Math.sqrt(objectA.material.friction * objectB.material.friction);
+            let staticCof = Math.sqrt(objectA.material.staticFriction * objectB.material.staticFriction);
+
+            let tangentVelocity = vec2.dot(relativeVelocity, tangent);
+            let normalForce = impulseMagnitude;
+            let frictionMagnitude: number;
+
+            let staticForce = Math.abs(tangentVelocity / massFactors);
+            if(staticForce < staticCof * normalForce){
+                frictionMagnitude = staticForce * objectA.deltaTime * objectA.deltaTime * objectA.deltaTime;
+            }
+            else{
+                frictionMagnitude = cof * normalForce;
+            }
+            if(tangentVelocity > 0){
+                frictionMagnitude *= -1;
+            }
+            objectA.applyForce(vec2.times(tangent, frictionMagnitude), contactA); 
+            objectB.applyForce(vec2.times(tangent, -frictionMagnitude), contactB);
+
+        }
+    }
+}

package/tsconfig.json

@@ -0,0 +1,10 @@
+{
+  "compilerOptions": {
+    "outDir": "./dist",
+    "allowJs": true,
+    "target": "es6",
+    "moduleResolution": "NodeNext",
+    "module": "NodeNext"
+
+  },
+}

package/world.ts

@@ -0,0 +1,126 @@
+import { Collection, GameObject, PhysicsObject } from "./body.js";
+import { Rotation, vec2 } from "../vec2/calc.js";
+import { Collision, Contact } from "./collision.js";
+import { Constraint } from "./constraints/constraint.js";
+import { Shape } from "./geometry.js";
+import { Solver } from "./solver.js";
+
+export class World{
+    root: Collection;
+    gravity: vec2 = new vec2(0, 0);
+    
+    solver: Solver = new Solver();
+    preSolve: Function;
+    constraints: Constraint[] = [];
+    
+
+    constructor(...objects: GameObject[]){
+        this.root = new Collection(objects);
+        this.preSolve = () => {};
+    }
+    addObjects(...objects: GameObject[]){
+            this.root.addObjects(...objects);
+    }
+    addConstraints(...constraints: Constraint[]){
+        this.constraints.push(...constraints);
+    }
+    step(dt: number, substeps: number){
+        for(let i = 0; i < substeps; i++){
+            this.substep(dt/substeps, i);
+        }
+    }
+    substep(dt: number, stepCount: number){
+
+        let objects = this.root.getAllObjects();
+
+        //update objects positions based on velocity & acceleration
+        for(let i = 0; i < objects.length; i++){
+            let object = objects[i];
+            
+            
+            // //verlet integration: p(t + 1) = 2 * p(t) - p(t - 1) + a(t) * dt^2
+            // let newPosition = vec2.minus(object.position, object.lastPosition).multiplyBy(dt / object.deltaTime).add(object.position);
+            // if(object.inverseMass != 0){
+            //     newPosition.add(vec2.times(this.gravity, dt * dt));
+            // }
+
+            
+            // let newAngle = Rotation.new(((object.angle.angle - object.lastAngle.angle) * dt / object.deltaTime) + object.angle.angle);
+            if(object.inverseMass != 0){
+                object.acceleration.add(vec2.times(this.gravity, dt));
+            }
+
+            object.velocity = vec2.plus(object.velocity, object.acceleration);
+            object.angularVelocity = Rotation.plus(object.angularVelocity, object.angularAccerleration);
+
+            let newPosition = vec2.plus(object.position, vec2.times(object.velocity, dt));
+            let newAngle = Rotation.plus(object.angle, Rotation.times(object.angularVelocity, dt));
+
+            object.lastPosition = object.position.copy();
+            object.lastAngle = object.angle.copy();
+            
+            
+            object.position = newPosition;
+            object.angle = newAngle;
+            
+                
+
+            object.acceleration = vec2.zero();
+            object.angularAccerleration = Rotation.zero();
+
+            //console.log(object.deltaTime);
+            object.deltaTime = dt;
+            
+        }
+        this.preSolve(stepCount);
+        //check for collisions
+        let contacts: Contact[] = [];
+        for(let i = 0; i < objects.length; i++){
+            let objectA = objects[i];
+            for(let j = i + 1; j < objects.length; j++){
+                let objectB = objects[j];
+                if(objectA.inverseMass == 0 && objectB.inverseMass == 0){
+                    continue;
+                }
+                contacts.push(...Collision(objectA, objectB))
+            }
+        }
+        for(let i = 0; i < 3; i++){
+            //Apply collision impulse forces
+            this.solver.resolveVelocities(contacts);
+            for(let constraint of this.constraints){
+                constraint.solveVelocity(dt/3);
+            }
+            this.applyAccelerations(objects, dt);
+        }
+        //correct positions (stop colliding objects from overlapping)
+        this.solver.resolvePositions(contacts);
+        for(let constraint of this.constraints){
+            constraint.solvePosition(dt);
+        }
+
+    }
+    applyAccelerations(objects: PhysicsObject[], dt: number){
+        for(let i = 0; i < objects.length; i++){
+            objects[i].lastPosition.subtract(vec2.times(objects[i].acceleration, dt));
+            objects[i].lastAngle.subtract(Rotation.times(objects[i].angularAccerleration, dt));
+
+            objects[i].velocity.add(vec2.times(objects[i].acceleration, 1))
+            objects[i].angularVelocity.add(Rotation.times(objects[i].angularAccerleration, 1))
+            
+            objects[i].acceleration = vec2.zero();
+            objects[i].angularAccerleration = Rotation.zero();
+        }
+
+    }
+    testHitbox(hitbox: Shape){
+        let dummyObject = new PhysicsObject(vec2.zero(), hitbox, {skipCOMcalc: true});
+        let contacts: Contact[] = [];
+        for(let o of this.root.getAllObjects()){
+            contacts.push(...Collision(o, dummyObject));
+        }
+        return contacts;
+    }
+}
+
+