SwerveDriveAnimation

var drawPointer = function(x1, y1, x2, y2) {
    var dx = x2 - x1;
    var dy = y2 - y1;
    
    var scale = 13/sqrt(sq(dx) + sq(dy));
    
    line(x1, y1, x2, y2);
    line(x2, y2, x2 - dy * scale, y2 + dx * scale);
    line(x2, y2, x2 - dy * -scale, y2 + dx * -scale);
};

var origin = [200, 200];
var loc0 = [350, 100];
var loc1 = [50, 100];
var loc2 = [50, 300];
var loc3 = [350, 300];
var r0 = sqrt(sq(origin[0] - loc0[0]) + sq(origin[1] - loc0[1]));
var r1 = sqrt(sq(origin[0] - loc1[0]) + sq(origin[1] - loc1[1]));
var r2 = sqrt(sq(origin[0] - loc2[0]) + sq(origin[1] - loc2[1]));
var r3 = sqrt(sq(origin[0] - loc3[0]) + sq(origin[1] - loc3[1]));

var maxSpeed = 25;
var translation = [0, 0];
var rotation = 0;

var v0 = [(loc0[1] - origin[1]) * rotation, (loc0[0] - origin[0]) * -rotation];
var v1 = [(loc1[1] - origin[1]) * rotation, (loc1[0] - origin[0]) * -rotation];
var v2 = [(loc2[1] - origin[1]) * rotation, (loc2[0] - origin[0]) * -rotation];
var v3 = [(loc3[1] - origin[1]) * rotation, (loc3[0] - origin[0]) * -rotation];

v0[0] /= r0; v0[1] /= r0;
v1[0] /= r1; v1[1] /= r1;
v2[0] /= r2; v2[1] /= r2;
v3[0] /= r3; v3[1] /= r3;

v0[0] += translation[0]; v0[1] += translation[1];
v1[0] += translation[0]; v1[1] += translation[1];
v2[0] += translation[0]; v2[1] += translation[1];
v3[0] += translation[0]; v3[1] += translation[1];

strokeWeight(5);
drawPointer(loc0[0], loc0[1], loc0[0] + v0[0], loc0[1] + v0[1]);
drawPointer(loc1[0], loc1[1], loc1[0] + v1[0], loc1[1] + v1[1]);
drawPointer(loc2[0], loc2[1], loc2[0] + v2[0], loc2[1] + v2[1]);
drawPointer(loc3[0], loc3[1], loc3[0] + v3[0], loc3[1] + v3[1]);

strokeWeight(1); stroke(0, 64, 255);
// line(origin[0], origin[1], loc0[0], loc0[1]);
// line(origin[0], origin[1], loc1[0], loc1[1]);
// line(origin[0], origin[1], loc2[0], loc2[1]);
// line(origin[0], origin[1], loc3[0], loc3[1]);

println(v0[0] + ", " + v0[1]);
println(v1[0] + ", " + v1[1]);
println(v2[0] + ", " + v2[1]);
println(v3[0] + ", " + v3[1]);