public static c_matrix rotate(e_axis axis, e_angle angle)
{
c_matrix result = identity();
switch (axis)
{
case e_axis.x:
result.values[1, 1] = cosine(angle);
result.values[1, 2] = -sine(angle);
result.values[2, 1] = sine(angle);
result.values[2, 2] = cosine(angle);
break;
case e_axis.y:
result.values[0, 0] = cosine(angle);
result.values[0, 2] = sine(angle);
result.values[2, 0] = -sine(angle);
result.values[2, 2] = cosine(angle);
break;
case e_axis.z:
result.values[0, 0] = cosine(angle);
result.values[0, 1] = -sine(angle);
result.values[1, 0] = sine(angle);
result.values[1, 1] = cosine(angle);
break;
default: throw new Exception("invalid axis");
}
return(result);
}
public static c_matrix scale(c_vector s)
{
c_matrix result = identity();
result.values[0, 0] *= s.x;
result.values[1, 1] *= s.y;
result.values[2, 2] *= s.z;
return(result);
}
public static c_matrix invert_translation(c_matrix m)
{
c_matrix result = new c_matrix(m);
result.values[0, 3] = -m.values[0, 3];
result.values[1, 3] = -m.values[1, 3];
result.values[2, 3] = -m.values[2, 3];
return(result);
}
public static c_matrix translate(c_vector t)
{
c_matrix result = identity();
result.values[0, 3] += t.x;
result.values[1, 3] += t.y;
result.values[2, 3] += t.z;
return(result);
}
public c_matrix(c_matrix other)
{
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
values[row, col] = other.values[row, col];
}
}
}
public static c_matrix identity()
{
c_matrix result = new c_matrix();
result.values[0, 0] = 1;
result.values[1, 1] = 1;
result.values[2, 2] = 1;
result.values[3, 3] = 1;
return(result);
}
// create a new scanner that is equivalent to this, but with operation applied to all the objects.
private c_scanner apply(c_matrix operation)
{
c_vector result_scanner = operation.multiply(scanner);
c_vector[] result_beacons = new c_vector[beacons.Length];
for (int i = 0; i < beacons.Length; i++)
{
result_beacons[i] = operation.multiply(beacons[i]);
}
return(new c_scanner(result_scanner, result_beacons));
}
public c_matrix multiply(c_matrix other)
{
// result is performing other first, then this.
c_matrix result = new c_matrix();
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
int value = 0;
for (int i = 0; i < 4; i++)
{
value += this.values[row, i] * other.values[i, col];
}
result.values[row, col] = value;
}
}
return(result);
}
