#include "stuff.h"

float distance(float p1[3], float p2[3]) {
    return sqrt(((p1[0]-p2[0])*(p1[0]-p2[0]))
	+ ((p1[1]-p2[1])*(p1[1]-p2[1]))
	+ ((p1[2]-p2[2])*(p1[2]-p2[2])));
}

void updatePoint(float m[16], float p[3]){
    float result[4];
    
    result[0]=m[0]*p[0]+m[4]*p[1]+m[8]*p[2]+m[12];
    result[1]=m[1]*p[0]+m[5]*p[1]+m[9]*p[2]+m[13];
    result[2]=m[2]*p[0]+m[6]*p[1]+m[10]*p[2]+m[14];
    result[3]=m[3]*p[0]+m[7]*p[1]+m[11]*p[2]+m[15];
    p[0]=result[0]/result[3];
    p[1]=result[1]/result[3];
    p[2]=result[2]/result[3];
}