physics.sk

class Vector2D {
	var x double
	var y double

	def new {
		self.x = 0
		self.y = 0
	}

	def new(x double, y double) {
		self.x = x
		self.y = y
	}

	def add(v Vector2D) Vector2D {
		x += v.x
		y += v.y

		return self
	}

	def subtract(v Vector2D) Vector2D {
		x -= v.x
		y -= v.y

		return self
	}

	def multiply(v Vector2D) Vector2D {
		x *= v.x
		y *= v.y

		return self
	}

	def lerp Vector2D {
		y = -y

		return self
	}

	def getLength double {
		return Math.sqrt((x * x) + (y * y))
	}

	def dotProduct(v Vector2D) double {
		return x * v.x + y * v.y
	}

	def reset Vector2D {
		return reset(0, 0)
	}

	def reset(x double, y double) Vector2D {
		self.x = x
		self.y = y

		return self
	}
}

class Manifold {
	var a AABB
	var b AABB
	var penetration double
	var normal Vector2D

	def new { }
}

class AABB {
	var x double
	var y double
	var width double
	var height double

	var min Vector2D
	var max Vector2D

	var mass double
	var invmass double
	var restitution double
	var velocity Vector2D

	def new(x double, y double) {
		self.x = x
		self.y = y

		width = 50
		height = 50

		min = Vector2D.new
		max = Vector2D.new

		setMass(100)

		restitution = 1
		velocity = Vector2D.new(Math.random * 70, Math.random * 70)
	}

	def update(dt double) {
		x += velocity.x * dt
		y += velocity.y * dt

		if x < 0 {
			x = 0
			velocity.x = -velocity.x
		} else if x + width > worldWidth {
			x = worldWidth - width
			velocity.x = -velocity.x
		}

		if y < 0 {
			y = 0
			velocity.y = -velocity.y
		} else if y + height > worldHeight {
			y = worldHeight - height
			velocity.y = -velocity.y
		}

		velocity.x *= 0.9
		velocity.y *= 0.9

		min.reset(x, y)
		max.reset(x + width, y + height)
	}

	def setMass(newMass double) {
		mass = newMass

		if newMass <= 0 {
			invmass = 0
		} else {
			invmass = 1 / newMass
		}
	}

	def draw(ctx dynamic) {
		ctx.fillStyle = "rgba(0, 10, 150, 0.5)"
	    ctx.fillRect(x, y, width, height)
	}
}

def AABBvsAABB(a AABB, b AABB) bool {
	if a.max.x < b.min.x || a.min.x > b.max.x { return false }
	if a.max.y < b.min.y || a.min.y > b.max.y { return false }

	return true
}

var normal = Vector2D.new
var manifold = Manifold.new

def overlapAABB(a AABB, b AABB) Manifold {
	# Vector from A to B
	normal.reset(b.x - a.x, b.y - a.y)

	# Calculate half extents along x axis for each object
	var a_extent = (a.max.x - a.min.x) / 2
	var b_extent = (b.max.x - b.min.x) / 2

	# Calculate overlap on x axis
	var x_overlap = a_extent + b_extent - Math.abs( normal.x )

	# SAT test on x axis
	if x_overlap > 0 {
		a_extent = (a.max.y - a.min.y) / 2 # var
		b_extent = (b.max.y - b.min.y) / 2

		# Calculate overlap on y axis
		var y_overlap = a_extent + b_extent - Math.abs( normal.y )

		# SAT test on y axis
		if y_overlap > 0 {
			# Find out which axis is axis of least penetration
			if x_overlap < y_overlap {
				# Point towards B knowing that dist points from A to B
				if normal.x < 0 {
					manifold.normal = normal.reset(-1, 0)
				} else {
					manifold.normal = normal.reset(1, 0)
				}

				manifold.penetration = x_overlap

				return manifold
			} else {
				# Point toward B knowing that dist points from A to B
				if normal.y < 0 {
					manifold.normal = normal.reset(0, -1)
				} else {
					manifold.normal = normal.reset(0, 1)
				}

				manifold.penetration = y_overlap

				return manifold
			}
		}
	}

	return null
}

var rv = Vector2D.new
var _impulse = Vector2D.new

# var trac = 0
def resolveCollision(a AABB, b AABB, m Manifold) {
	# Calculate relative velocity
	rv.reset( b.velocity.x - a.velocity.x, b.velocity.y - a.velocity.y )

	# Calculate relative velocity in terms of the normal direction
    var velAlongNormal = rv.dotProduct(m.normal)

	# Do not resolve if velocities are separating
	if velAlongNormal > 0 {
		 return
	}

	# Calculate restitution
	var e = Math.min(a.restitution, b.restitution)

	# Calculate impulse scalar
	var j = -(1 + e) * velAlongNormal
	j /= a.invmass + b.invmass

	# Apply impulse
	_impulse.reset(m.normal.x * j, m.normal.y * j)

	a.velocity.x -= (a.invmass * _impulse.x)
	a.velocity.y -= (a.invmass * _impulse.y)

	b.velocity.x += (b.invmass * _impulse.x)
	b.velocity.y += (b.invmass * _impulse.y)
}

# APPLICATION
# -----------

var ctx dynamic
var trackRect dynamic
var numRects int = 3
var halfPI = Math.PI / 2
var pi2 = Math.PI * 2
var rad = Math.PI/180
var rects = List<AABB>.new
var _mark int = 0
var elapsed = 0.0

var worldWidth = 400
var worldHeight = 300
var kb = input.Keyboard.new

def tick {
	ctx.clearRect(0, 0, worldWidth, worldHeight)

	rects[0].velocity = Vector2D.new

	if kb.down(Key.Left) {
		rects[0].velocity.x -= 100
	}

	if kb.down(Key.Right) {
		rects[0].velocity.x += 100
	}

	if kb.down(Key.Up) {
		rects[0].velocity.y -= 100
	}

	if kb.down(Key.Down) {
		rects[0].velocity.y += 100
	}

	var now = dynamic.Date.now()
	var dt = now - _mark

	_mark = now
	elapsed = dt * 0.001

	for i in 0 .. numRects {
		rects[i].update(elapsed)
	}

	for i in 0 .. numRects {
		var a = rects[i]

		for j in i + 1 .. numRects {
			var b = rects[j]

			var m = overlapAABB(a, b)
			if m != null {
                resolveCollision( a, b, m )
			}
		}
	}

	for i in 0 .. numRects {
		rects[i].draw(ctx)
	}

	dynamic.window.requestAnimationFrame(=> tick)
}

@entry
def main {
	var canvas = dynamic.document.getElementsByTagName("canvas")[0]
	canvas.width = worldWidth
	canvas.height = worldHeight
	ctx = canvas.getContext("2d")

	rects.append(AABB.new(0, 80))
	rects[0].velocity = Vector2D.new(0, 0)

	rects.append(AABB.new(150, 80))
	rects[1].setMass(200)
	rects[1].velocity = Vector2D.new

	rects.append(AABB.new(250, 80))
	rects[2].setMass(200)
	rects[2].velocity = Vector2D.new

	_mark = dynamic.Date.now()
	tick
}