public Vector Read()
{
lock (inputLock)
{
Vector result = delta;
delta = new Vector(0.0f, 0.0f, 0.0f);
return result;
}
}
public static Matrix Rotate(float angle, Vector axis)
{
float c = (float)Math.Cos(angle);
float s = (float)Math.Sin(angle);
float x = axis.X;
float y = axis.Y;
float z = axis.Z;
return new Matrix(new float[]
{
x*x*(1-c)+c, y*x*(1-c)+z*s, z*x*(1-c)-y*s, 0,
x*y*(1-c)-z*s, y*y*(1-c)+c, z*y*(1-c)+x*s, 0,
x*z*(1-c)+y*s, y*z*(1-c)-x*s, z*z*(1-c)+c, 0,
0, 0, 0, 1
});
}
public void GlobalTranslate(Vector v)
{
CoordinateSystem = CoordinateSystem.GloballyTranslated(v);
}
public static float Dot(Vector u, Vector v)
{
return u.X*v.X + u.Y*v.Y + u.Z*v.Z;
}
public static Vector Cross(Vector u, Vector v)
{
return new Vector(
u.Y*v.Z-u.Z*v.Y,
u.Z*v.X-u.X*v.Z,
u.X*v.Y-u.Y*v.X
);
}
public static Matrix Scale(Vector s)
{
return new Matrix(new float[]
{
s.X, 0, 0, 0,
0, s.Y, 0, 0,
0, 0, s.Z, 0,
0, 0, 0, 1
});
}
public static Quaternion RotateQ(float angle, Vector axis)
{
axis = axis.Normalized();
angle /= 2.0f;
float s = (float)Math.Sin(angle);
float c = (float)Math.Cos(angle);
return new Quaternion(axis.X * s, axis.Y * s, axis.Z * s, c);
}
public Vector ToLocal(Vector v)
{
return Rotation.Conjugate() * v;
}
public Mouse()
{
inputLock = new object();
delta = Vector.Zero;
}
public Vector ToGlobal(Vector v)
{
return Rotation * v;
}
private void ProcessInput()
{
Vector forward = camera.CoordinateSystem.ToGlobal(Vector.UnitZ);
forward = new Vector(-forward.X, 0.0f, -forward.Z);
Vector d = mouse.Read();
if (d.Y != 0.0f) camera.LocalRotate( d.Y / 200.0f, Vector.UnitX);
if (d.X != 0.0f) camera.LocalRotate(-d.X / 200.0f, camera.CoordinateSystem.ToLocal(Vector.UnitY));
TimeSpan t;
t = keyboard.Read(Keys.W); if (t.TotalSeconds > 0.0f) camera.GlobalTranslate(forward * (float)t.TotalSeconds * 3.0f);
t = keyboard.Read(Keys.S); if (t.TotalSeconds > 0.0f) camera.GlobalTranslate(forward * (float)t.TotalSeconds * -3.0f);
t = keyboard.Read(Keys.D); if (t.TotalSeconds > 0.0f) camera.LocalTranslate(Vector.UnitX * (float)t.TotalSeconds * 3.0f);
t = keyboard.Read(Keys.A); if (t.TotalSeconds > 0.0f) camera.LocalTranslate(Vector.UnitX * (float)t.TotalSeconds * -3.0f);
t = keyboard.Read(Keys.Space); if (t.TotalSeconds > 0.0f) camera.GlobalTranslate(Vector.UnitY * (float)t.TotalSeconds * 3.0f);
t = keyboard.Read(Keys.Z); if (t.TotalSeconds > 0.0f) camera.GlobalTranslate(Vector.UnitY * (float)t.TotalSeconds * -3.0f);
}
// Rotate(angle, axis, point)
public CoordinateSystem LocallyTranslated(Vector v)
{
return new CoordinateSystem(Rotation, Origin + (Rotation * v));
}
public CoordinateSystem LocallyRotated(float angle, Vector axis)
{
return LocallyRotated(Transform.RotateQ(angle, axis));
}
public CoordinateSystem GloballyTranslated(Vector v)
{
return new CoordinateSystem(Rotation, Origin + v);
}
public void LocalRotate(float angle, Vector axis)
{
CoordinateSystem = CoordinateSystem.LocallyRotated(angle, axis);
}
public static Matrix Translate(Vector t)
{
return new Matrix(new float[]
{
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
t.X, t.Y, t.Z, 1
});
}
public void LocalTranslate(Vector v)
{
CoordinateSystem = CoordinateSystem.LocallyTranslated(v);
}
private static void AssertVectorsAreEqual(Vector u, Vector v)
{
Assert.AreEqual(u.X, v.X, 0.0001);
Assert.AreEqual(u.Y, v.Y, 0.0001);
Assert.AreEqual(u.Z, v.Z, 0.0001);
}