gered/Blarg.GameFramework
Archived
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

add a bunch of ported math classes

Browse source
This commit is contained in:
Gered 2013-08-14 19:26:42 -04:00
parent bfcee93006
commit de75de9d83
20 changed files with 5694 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
Blarg.GameFramework/Blarg.GameFramework.csproj
View file

@ -56,6 +56,25 @@
<Compile Include="IO\IFileSystem.cs" />
<Compile Include="Math\Rect.cs" />
<Compile Include="Math\Circle.cs" />
<Compile Include="Math\BoundingBox.cs" />
<Compile Include="Math\Vector3.cs" />
<Compile Include="Math\Point3.cs" />
<Compile Include="Math\BoundingSphere.cs" />
<Compile Include="Math\Vector2.cs" />
<Compile Include="Math\Point2.cs" />
<Compile Include="Math\Vector4.cs" />
<Compile Include="Math\RectF.cs" />
<Compile Include="Math\Quaternion.cs" />
<Compile Include="Math\Matrix4x4.cs" />
<Compile Include="Math\Matrix3x3.cs" />
<Compile Include="Math\Plane.cs" />
<Compile Include="Math\MathConstants.cs" />
<Compile Include="Math\LineSegment.cs" />
<Compile Include="Math\Ray.cs" />
<Compile Include="Math\MathHelpers.cs" />
<Compile Include="Math\Transformation.cs" />
<Compile Include="Math\SweptEllipsoidCollisionPacket.cs" />
<Compile Include="Math\IntersectionTester.cs" />
</ItemGroup>
<Import Project="$(MSBuildExtensionsPath32)\Microsoft\Portable\$(TargetFrameworkVersion)\Microsoft.Portable.CSharp.targets" />
<ItemGroup>

164
Blarg.GameFramework/Math/BoundingBox.cs Normal file
View file

@ -0,0 +1,164 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct BoundingBox : IEquatable<BoundingBox>
{
public Vector3 Min;
public Vector3 Max;
public float Width
{
get { return Math.Abs(Max.X - Min.X); }
}
public float Height
{
get { return Math.Abs(Max.Y - Min.Y); }
}
public float Depth
{
get { return Math.Abs(Max.Z - Min.Z); }
}
public BoundingBox(Vector3 min, Vector3 max)
: this(ref min, ref max)
{
}
public BoundingBox(ref Vector3 min, ref Vector3 max)
{
Min = min;
Max = max;
}
public BoundingBox(float minX, float minY, float minZ, float maxX, float maxY, float maxZ)
{
Min.X = minX;
Min.Y = minY;
Min.Z = minZ;
Max.X = maxX;
Max.Y = maxY;
Max.Z = maxZ;
}
public BoundingBox(Vector3 center, float halfWidth)
: this(ref center, halfWidth)
{
}
public BoundingBox(ref Vector3 center, float halfWidth)
{
Min.X = center.X - halfWidth;
Min.Y = center.Y - halfWidth;
Min.Z = center.Z - halfWidth;
Max.X = center.X + halfWidth;
Max.Y = center.Y + halfWidth;
Max.Z = center.Z + halfWidth;
}
public BoundingBox(Vector3[] vertices)
{
float minX = 0.0f;
float minY = 0.0f;
float minZ = 0.0f;
float maxX = 0.0f;
float maxY = 0.0f;
float maxZ = 0.0f;
for (int i = 0; i < vertices.Length; ++i)
{
minX = Math.Min(vertices[i].X, minX);
minY = Math.Min(vertices[i].Y, minY);
minZ = Math.Min(vertices[i].Z, minZ);
maxX = Math.Max(vertices[i].X, maxX);
maxY = Math.Max(vertices[i].Y, maxY);
maxZ = Math.Max(vertices[i].Z, maxZ);
}
Min.X = minX;
Min.Y = minY;
Min.Z = minZ;
Max.X = maxX;
Max.Y = maxY;
Max.Z = maxZ;
}
public static float GetSquaredDistanceFromPointToBox(Vector3 point, BoundingBox box)
{
return GetSquaredDistanceFromPointToBox(ref point, ref box);
}
public static float GetSquaredDistanceFromPointToBox(ref Vector3 point, ref BoundingBox box)
{
float distanceSq = 0.0f;
float v;
v = point.X;
if (v < box.Min.X)
distanceSq += (box.Min.X - v) * (box.Min.X - v);
if (v > box.Max.X)
distanceSq += (v - box.Max.X) * (v - box.Max.X);
v = point.Y;
if (v < box.Min.Y)
distanceSq += (box.Min.Y - v) * (box.Min.Y - v);
if (v > box.Max.Y)
distanceSq += (v - box.Max.Y) * (v - box.Max.Y);
v = point.Z;
if (v < box.Min.Z)
distanceSq += (box.Min.Z - v) * (box.Min.Z - v);
if (v > box.Max.Z)
distanceSq += (v - box.Max.Z) * (v - box.Max.Z);
return distanceSq;
}
public static float GetDistanceFromPointToBox(Vector3 point, BoundingBox box)
{
return GetDistanceFromPointToBox(ref point, ref box);
}
public static float GetDistanceFromPointToBox(ref Vector3 point, ref BoundingBox box)
{
return (float)Math.Sqrt(GetSquaredDistanceFromPointToBox(ref point, ref box));
}
public static bool operator ==(BoundingBox left, BoundingBox right)
{
return left.Equals(right);
}
public static bool operator !=(BoundingBox left, BoundingBox right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is BoundingBox)
return this.Equals((BoundingBox)obj);
else
return false;
}
public bool Equals(BoundingBox other)
{
return (Min == other.Min && Max == other.Max);
}
public override int GetHashCode()
{
return Min.GetHashCode() ^ Max.GetHashCode();
}
public override string ToString()
{
return String.Format("{{{0}, {1}}}", Min, Max);
}
}
}

134
Blarg.GameFramework/Math/BoundingSphere.cs Normal file
View file

@ -0,0 +1,134 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct BoundingSphere : IEquatable<BoundingSphere>
{
public Vector3 Center;
public float Radius;
public BoundingSphere(Vector3 center, float radius)
: this(ref center, radius)
{
}
public BoundingSphere(ref Vector3 center, float radius)
{
Center = center;
Radius = radius;
}
public BoundingSphere(float centerX, float centerY, float centerZ, float radius)
{
Center.X = centerX;
Center.Y = centerY;
Center.Z = centerZ;
Radius = radius;
}
public BoundingSphere(Vector3[] vertices)
{
int min;
int max;
int minX = 0;
int minY = 0;
int minZ = 0;
int maxX = 0;
int maxY = 0;
int maxZ = 0;
// find min & max points for x, y and z
for (int i = 0; i < vertices.Length; ++i)
{
if (vertices[i].X < vertices[minX].X)
minX = i;
if (vertices[i].X > vertices[maxX].X)
maxX = i;
if (vertices[i].Y < vertices[minY].Y)
minY = i;
if (vertices[i].Y > vertices[maxY].Y)
maxY = i;
if (vertices[i].Z < vertices[minZ].Z)
minZ = i;
if (vertices[i].Z > vertices[maxZ].Z)
maxZ = i;
}
// distances between these extremes for x, y and z
float distanceSqX = Vector3.Dot(vertices[maxX] - vertices[minX], vertices[maxX] - vertices[minX]);
float distanceSqY = Vector3.Dot(vertices[maxY] - vertices[minY], vertices[maxY] - vertices[minY]);
float distanceSqZ = Vector3.Dot(vertices[maxZ] - vertices[minZ], vertices[maxZ] - vertices[minZ]);
// get the pair of points representing the most distance points from each other
min = minX;
max = maxX;
if (distanceSqY > distanceSqX && distanceSqY > distanceSqZ)
{
min = minY;
max = maxY;
}
if (distanceSqZ > distanceSqX && distanceSqZ > distanceSqY)
{
min = minZ;
max = maxZ;
}
// we now have enough info to set the initial sphere properties
Center = (vertices[min] + vertices[max]) / 2.0f;
Radius = (float)Math.Sqrt(Vector3.Dot(vertices[max] - Center, vertices[max] - Center));
// now expand the sphere to make sure it encompasses all the points (if it doesn't already)
Vector3 d;
for (int i = 0; i < vertices.Length; ++i)
{
d = vertices[i] - Center;
float distanceSq = Vector3.Dot(d, d);
if (distanceSq > (Radius * Radius))
{
float distance = (float)Math.Sqrt(distanceSq);
float newRadius = (Radius + distance) * 0.5f;
float k = (newRadius - Radius) / distance;
Radius = newRadius;
d = d * k;
Center = Center + d;
}
}
}
public static bool operator ==(BoundingSphere left, BoundingSphere right)
{
return left.Equals(right);
}
public static bool operator !=(BoundingSphere left, BoundingSphere right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is BoundingSphere)
return this.Equals((BoundingSphere)obj);
else
return false;
}
public bool Equals(BoundingSphere other)
{
return (Center == other.Center && Radius == other.Radius);
}
public override int GetHashCode()
{
return Center.GetHashCode() ^ Radius.GetHashCode();
}
public override string ToString()
{
return String.Format("{{Center:{0} Radius:{1}}}", Center, Radius);
}
}
}

527
Blarg.GameFramework/Math/IntersectionTester.cs Normal file
View file

@ -0,0 +1,527 @@
using System;
namespace Blarg.GameFramework
{
public static class IntersectionTester
{
public static bool Test(ref BoundingBox box, ref Vector3 point)
{
if ((point.X >= box.Min.X && point.X <= box.Max.X) &&
(point.Y >= box.Min.Y && point.Y <= box.Max.Y) &&
(point.Z >= box.Min.Z && point.Z <= box.Max.Z))
return true;
else
return false;
}
public static bool Test(ref BoundingSphere sphere, ref Vector3 point)
{
if ((float)Math.Abs(Vector3.Distance(ref point, ref sphere.Center)) < sphere.Radius)
return true;
else
return false;
}
public static bool Test(ref BoundingBox box, Vector3[] vertices, ref Vector3 outFirstIntersection)
{
for (int i = 0; i < vertices.Length; ++i)
{
if ((vertices[i].X >= box.Min.X && vertices[i].X <= box.Max.X) &&
(vertices[i].Y >= box.Min.Y && vertices[i].Y <= box.Max.Y) &&
(vertices[i].Z >= box.Min.Z && vertices[i].Z <= box.Max.Z))
{
outFirstIntersection = vertices[i];
return true;
}
}
return false;
}
public static bool Test(ref BoundingSphere sphere, Vector3[] vertices, ref Vector3 outFirstIntersection)
{
for (int i = 0; i < vertices.Length; ++i)
{
if ((float)Math.Abs(Vector3.Distance(ref vertices[i], ref sphere.Center)) < sphere.Radius)
{
outFirstIntersection = vertices[i];
return true;
}
}
return false;
}
public static bool Test(ref BoundingBox a, ref BoundingBox b)
{
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;
if (a.Max.Z < b.Min.Z || a.Min.Z > b.Max.Z)
return false;
return true;
}
public static bool Test(ref BoundingSphere a, ref BoundingSphere b)
{
Vector3 temp = a.Center - b.Center;
float distanceSquared = Vector3.Dot(temp, temp);
float radiusSum = a.Radius + b.Radius;
if (distanceSquared <= radiusSum * radiusSum)
return true;
else
return false;
}
public static bool Test(ref BoundingSphere sphere, ref Plane plane)
{
float distance = Vector3.Dot(sphere.Center, plane.Normal) - plane.D;
if ((float)Math.Abs(distance) <= sphere.Radius)
return true;
else
return false;
}
public static bool Test(ref BoundingBox box, ref Plane plane)
{
Vector3 temp1 = (box.Max + box.Min) / 2.0f;
Vector3 temp2 = box.Max - temp1;
float radius = (temp2.X * (float)Math.Abs(plane.Normal.X)) + (temp2.Y * (float)Math.Abs(plane.Normal.Y)) + (temp2.Z * (float)Math.Abs(plane.Normal.Z));
float distance = Vector3.Dot(plane.Normal, temp1) - plane.D;
if ((float)Math.Abs(distance) <= radius)
return true;
else
return true;
}
public static bool Test(ref Ray ray, ref Plane plane, ref Vector3 outIntersection)
{
float denominator = Vector3.Dot(ray.Direction, plane.Normal);
if (denominator == 0.0f)
return false;
float t = ((-plane.D - Vector3.Dot(ray.Position, plane.Normal)) / denominator);
if (t < 0.0f)
return false;
Vector3 temp1 = ray.GetPositionAt(t);
outIntersection.X = temp1.X;
outIntersection.Y = temp1.Y;
outIntersection.Z = temp1.Z;
return true;
}
public static bool Test(ref Ray ray, ref BoundingSphere sphere, ref Vector3 outFirstIntersection)
{
Vector3 temp1 = ray.Position - sphere.Center;
float b = Vector3.Dot(temp1, ray.Direction);
float c = Vector3.Dot(temp1, temp1) - (sphere.Radius * sphere.Radius);
if (c > 0.0f && b > 0.0f)
return false;
float discriminant = b * b - c;
if (discriminant < 0.0f)
return false;
float t = -b - (float)Math.Sqrt(discriminant);
if (t < 0.0f)
t = 0.0f;
Vector3 temp2 = ray.GetPositionAt(t);
outFirstIntersection.X = temp2.X;
outFirstIntersection.Y = temp2.Y;
outFirstIntersection.Z = temp2.Z;
return true;
}
public static bool Test(ref Ray ray, ref BoundingBox box, ref Vector3 outFirstIntersection)
{
float tmin = 0.0f;
float tmax = float.MaxValue;
if ((float)Math.Abs(ray.Direction.X) < float.Epsilon)
{
if (ray.Position.X < box.Min.X || ray.Position.X > box.Max.X)
return false;
}
else
{
float invD = 1.0f / ray.Direction.X;
float t1 = (box.Min.X - ray.Position.X) * invD;
float t2 = (box.Max.X - ray.Position.X) * invD;
if (t1 > t2)
{
float tswap = t1;
t1 = t2;
t2 = tswap;
}
tmin = Math.Max(tmin, t1);
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
}
if ((float)Math.Abs(ray.Direction.Y) < float.Epsilon)
{
if (ray.Position.Y < box.Min.Y || ray.Position.Y > box.Max.Y)
return false;
}
else
{
float invD = 1.0f / ray.Direction.Y;
float t1 = (box.Min.Y - ray.Position.Y) * invD;
float t2 = (box.Max.Y - ray.Position.Y) * invD;
if (t1 > t2)
{
float tswap = t1;
t1 = t2;
t2 = tswap;
}
tmin = Math.Max(tmin, t1);
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
}
if ((float)Math.Abs(ray.Direction.Z) < float.Epsilon)
{
if (ray.Position.Z < box.Min.Z || ray.Position.Z > box.Max.Z)
return false;
}
else
{
float invD = 1.0f / ray.Direction.Z;
float t1 = (box.Min.Z - ray.Position.Z) * invD;
float t2 = (box.Max.Z - ray.Position.Z) * invD;
if (t1 > t2)
{
float tswap = t1;
t1 = t2;
t2 = tswap;
}
tmin = Math.Max(tmin, t1);
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
return false;
}
Vector3 temp1 = ray.GetPositionAt(tmin);
outFirstIntersection.X = temp1.X;
outFirstIntersection.Y = temp1.Y;
outFirstIntersection.Z = temp1.Z;
return true;
}
public static bool Test(ref BoundingBox box, ref BoundingSphere sphere)
{
float distanceSq = BoundingBox.GetSquaredDistanceFromPointToBox(ref sphere.Center, ref box);
if (distanceSq <= (sphere.Radius * sphere.Radius))
return true;
else
return false;
}
public static bool Test(ref Ray ray, ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 outIntersection)
{
float r, num1, num2;
Vector3 temp1 = Vector3.Cross(b - a, c - a);
if (temp1.X == 0.0f && temp1.Y == 0.0f && temp1.Z == 0.0f)
return false;
Vector3 temp2 = ray.Position - a;
num1 = -Vector3.Dot(temp1, temp2);
num2 = Vector3.Dot(temp1, ray.Direction);
if ((float)Math.Abs(num2) < float.Epsilon)
{
if (num1 == 0.0f)
{
outIntersection = ray.Position;
return true;
}
else
return false;
}
r = num1 / num2;
if (r < 0.0f)
return false;
Vector3 temp3 = ray.GetPositionAt(r);
if (Test(ref temp3, ref a, ref b, ref c))
{
outIntersection = temp3;
return true;
}
else
return false;
}
public static bool Test(ref Vector3 point, ref Vector3 a, ref Vector3 b, ref Vector3 c)
{
Vector3 v0 = c - a;
Vector3 v1 = b - a;
Vector3 v2 = point - a;
float dot00 = (v0.X * v0.X) + (v0.Y * v0.Y) + (v0.Z * v0.Z);
float dot01 = (v0.X * v1.X) + (v0.Y * v1.Y) + (v0.Z * v1.Z);
float dot02 = (v0.X * v2.X) + (v0.Y * v2.Y) + (v0.Z * v2.Z);
float dot11 = (v1.X * v1.X) + (v1.Y * v1.Y) + (v1.Z * v1.Z);
float dot12 = (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z);
float denom = dot00 * dot11 - dot01 * dot01;
if (denom == 0)
return false;
float u = (dot11 * dot02 - dot01 * dot12) / denom;
float v = (dot00 * dot12 - dot01 * dot02) / denom;
if (u >= 0 && v >= 0 && u + v <= 1)
return true;
else
return false;
}
public static bool Test(ref SweptEllipsoidCollisionPacket packet, ref Vector3 v1, ref Vector3 v2, ref Vector3 v3)
{
bool foundCollision = false;
Vector3 p1;
Vector3 p2;
Vector3 p3;
Vector3.Divide(ref v1, ref packet.EllipsoidRadius, out p1);
Vector3.Divide(ref v2, ref packet.EllipsoidRadius, out p2);
Vector3.Divide(ref v3, ref packet.EllipsoidRadius, out p3);
var trianglePlane = new Plane(ref p1, ref p2, ref p3);
// Is the triangle front-facing to the entity's velocity?
if (Plane.IsFrontFacingTo(ref trianglePlane, ref packet.esNormalizedVelocity))
{
float t0;
float t1;
bool embeddedInPlane = false;
float distToTrianglePlane = Plane.DistanceBetween(ref trianglePlane, ref packet.esPosition);
float normalDotVelocity = Vector3.Dot(trianglePlane.Normal, packet.esVelocity);
// Is the sphere travelling parallel to the plane?
if (normalDotVelocity == 0.0f)
{
if ((float)Math.Abs(distToTrianglePlane) >= 1.0f)
{
// Sphere is not embedded in the plane, no collision possible
return false;
}
else
{
// Sphere is embedded in the plane, it intersects throughout the whole time period
embeddedInPlane = true;
t0 = 0.0f;
t1 = 1.0f;
}
}
else
{
// Not travelling parallel to the plane
t0 = (-1.0f - distToTrianglePlane) / normalDotVelocity;
t1 = (1.0f - distToTrianglePlane) / normalDotVelocity;
// Swap so t0 < t1
if (t0 > t1)
{
float temp = t1;
t1 = t0;
t0 = temp;
}
// Check that at least one result is within range
if (t0 > 1.0f || t1 < 0.0f)
{
// Both values outside the range [0,1], no collision possible
return false;
}
t0 = MathHelpers.Clamp(t0, 0.0f, 1.0f);
t1 = MathHelpers.Clamp(t1, 0.0f, 1.0f);
}
// At this point, we have two time values (t0, t1) between which the
// swept sphere intersects with the triangle plane
Vector3 collisionPoint = new Vector3();
float t = 1.0f;
// First, check for a collision inside the triangle. This will happen
// at time t0 if at all as this is when the sphere rests on the front
// side of the triangle plane.
if (!embeddedInPlane)
{
Vector3 planeIntersectionPoint = (packet.esPosition - trianglePlane.Normal) + packet.esVelocity * t0;
if (Test(ref planeIntersectionPoint, ref p1, ref p2, ref p3))
{
foundCollision = true;
t = t0;
collisionPoint = planeIntersectionPoint;
}
}
// If we haven't found a collision at this point, we need to check the
// points and edges of the triangle
if (foundCollision == false)
{
Vector3 velocity = packet.esVelocity;
Vector3 basePoint = packet.esPosition;
float velocitySquaredLength = velocity.LengthSquared;
float a, b, c;
float newT = 0.0f;
// For each vertex or edge, we have a quadratic equation to be solved
// Check against the points first
a = velocitySquaredLength;
// P1
b = 2.0f * Vector3.Dot(velocity, basePoint - p1);
c = (p1 - basePoint).LengthSquared - 1.0f;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
t = newT;
foundCollision = true;
collisionPoint = p1;
}
// P2
b = 2.0f * Vector3.Dot(velocity, basePoint - p2);
c = (p2 - basePoint).LengthSquared - 1.0f;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
t = newT;
foundCollision = true;
collisionPoint = p2;
}
// P3
b = 2.0f * Vector3.Dot(velocity, basePoint - p3);
c = (p3 - basePoint).LengthSquared - 1.0f;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
t = newT;
foundCollision = true;
collisionPoint = p3;
}
// Now check against the edges
// P1 -> P2
Vector3 edge = p2 - p1;
Vector3 baseToVertex = p1 - basePoint;
float edgeSquaredLength = edge.LengthSquared;
float edgeDotVelocity = Vector3.Dot(edge, velocity);
float edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
// Check if intersection is within line segment
float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
if (f >= 0.0f && f <= 1.0f)
{
// Intersection took place within the segment
t = newT;
foundCollision = true;
collisionPoint = p1 + edge * f;
}
}
// P2 -> P3
edge = p3 - p2;
baseToVertex = p2 - basePoint;
edgeSquaredLength = edge.LengthSquared;
edgeDotVelocity = Vector3.Dot(edge, velocity);
edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
// Check if intersection is within line segment
float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
if (f >= 0.0f && f <= 1.0f)
{
// Intersection took place within the segment
t = newT;
foundCollision = true;
collisionPoint = p2 + edge * f;
}
}
// P3 -> P1
edge = p1 - p3;
baseToVertex = p3 - basePoint;
edgeSquaredLength = edge.LengthSquared;
edgeDotVelocity = Vector3.Dot(edge, velocity);
edgeDotBaseToVertex = Vector3.Dot(edge, baseToVertex);
a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
b = edgeSquaredLength * (2.0f * Vector3.Dot(velocity, baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex;
c = edgeSquaredLength * (1.0f - baseToVertex.LengthSquared) + edgeDotBaseToVertex * edgeDotBaseToVertex;
if (MathHelpers.GetLowestQuadraticRoot(a, b, c, t, out newT))
{
// Check if intersection is within line segment
float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
if (f >= 0.0f && f <= 1.0f)
{
// Intersection took place within the segment
t = newT;
foundCollision = true;
collisionPoint = p3 + edge * f;
}
}
}
// Set result of test
if (foundCollision == true)
{
float distanceToCollision = t * packet.esVelocity.Length;
// Does this triangle qualify for the closest collision?
if (packet.FoundCollision == false || distanceToCollision < packet.NearestDistance)
{
packet.NearestDistance = distanceToCollision;
packet.esIntersectionPoint = collisionPoint;
packet.FoundCollision = true;
}
}
}
return foundCollision;
}
}
}

77
Blarg.GameFramework/Math/LineSegment.cs Normal file
View file

@ -0,0 +1,77 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct LineSegment : IEquatable<LineSegment>
{
public Vector3 A;
public Vector3 B;
public LineSegment(Vector3 a, Vector3 b)
: this(ref a, ref b)
{
}
public LineSegment(ref Vector3 a, ref Vector3 b)
{
A = a;
B = b;
}
public LineSegment(float ax, float ay, float az, float bx, float by, float bz)
{
A.X = ax;
A.Y = ay;
A.Z = az;
B.X = bx;
B.Y = by;
B.Z = bz;
}
public LineSegment(Ray ray, float length)
: this(ref ray, length)
{
}
public LineSegment(ref Ray ray, float length)
{
A = ray.Position;
B = ray.GetPositionAt(length);
}
public static bool operator ==(LineSegment left, LineSegment right)
{
return left.Equals(right);
}
public static bool operator !=(LineSegment left, LineSegment right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is LineSegment)
return this.Equals((LineSegment)obj);
else
return false;
}
public bool Equals(LineSegment other)
{
return (A == other.A && B == other.B);
}
public override int GetHashCode()
{
return A.GetHashCode() ^ B.GetHashCode();
}
public override string ToString()
{
return String.Format("{{{0}-{1}}}", A, B);
}
}
}

28
Blarg.GameFramework/Math/MathConstants.cs Normal file
View file

@ -0,0 +1,28 @@
using System;
namespace Blarg.GameFramework
{
public static class MathConstants
{
public const float Pi = 3.14159274f; // 180 degrees
public const float HalfPi = Pi / 2.0f; // 90 degrees
public const float QuarterPi = Pi / 4.0f; // 45 degrees
public const float TwoPi = Pi * 2.0f; // 360 degrees
public const float PiOver180 = Pi / 180.0f;
public const float Radians0 = 0.0f;
public const float Radians45 = Pi / 4.0f;
public const float Radians90 = Pi / 2.0f;
public const float Radians135 = (3.0f * Pi) / 4.0f;
public const float Radians180 = Pi;
public const float Radians225 = (5.0f * Pi) / 4.0f;
public const float Radians270 = (3.0f * Pi) / 2.0f;
public const float Radians315 = (7.0f * Pi) / 4.0f;
public const float Radians360 = Pi * 2.0f;
public const float Radians60 = (60.0f * MathConstants.PiOver180);
public const float Tolerance = 0.000001f;
}
}

315
Blarg.GameFramework/Math/MathHelpers.cs Normal file
View file

@ -0,0 +1,315 @@
using System;
namespace Blarg.GameFramework
{
public static class MathHelpers
{
/// <summary>
/// Converts coordinates from spherical to cartesian.
/// </summary>
/// <param name="radius">distance from the origin "O" to the point "P" (the "r" value)</param>
/// <param name="angleTheta">the angle (in radians) between the zenith direction and the line segment OP</param>
/// <param name="angleRho">the signed angle (in radians) measured from the azimuth reference direction to the orthogonal projection of the line segment OP on the reference plane</param>
/// <param name="cartesianCoords">the output cartesian coordinates</param>
public static void GetCartesianCoordsFromSpherical(float radius, float angleTheta, float angleRho, out Vector3 cartesianCoords)
{
cartesianCoords.X = radius * (float)Math.Sin(angleTheta) * (float)Math.Sin(angleRho);
cartesianCoords.Y = radius * (float)Math.Cos(angleTheta);
cartesianCoords.Z = radius * (float)Math.Sin(angleTheta) * (float)Math.Cos(angleRho);
}
/// <summary>
/// Converts an angle around the Y axis to a direction vector that
/// lies flat on the XZ plane. Note that the angle is offset so that 0 degrees
/// points directly down the +Y axis on a 2D cartesian grid, instead of the +X
/// axis as one might expect.
/// </summary>
/// <returns>direction vector on the XZ plane</returns>
/// <param name="angle">the Y axis angle (in radians)</param>
public static Vector3 GetDirectionFromYAxisOrientation(float angle)
{
Vector3 facing;
facing.Y = 0.0f;
// TODO: perhaps the whole "90 degree offset" thing we're doing below
// is too scenario-specific. maybe have an overload of this function
// which accepts an offset angle parameter?
//
// GetPointOnCircle() returns an angle based on a 2D cartesian grid where
// 0 degrees points in the +X direction. We want it to point in the
// +Y direction (2D cartesian Y = our 3D Z), so we offset our angle by 90
// degrees before calling it to get the intended result
float adjustedAngle = RolloverClamp(angle - MathConstants.Radians90, MathConstants.Radians0, MathConstants.Radians360);
GetPointOnCircle(1.0f, adjustedAngle, out facing.X, out facing.Z);
return facing;
}
/// <summary>
/// Converts euler angles to an equivalent direction vector. Note that this just
/// uses one of many other ways to do this (the others using any other rotation
/// order).
/// </summary>
/// <returns>direction vector</returns>
/// <param name="yaw">the yaw rotation angle (in radians)</param>
/// <param name="pitch">the pitch rotation angle (in radians)</param>
public static Vector3 GetDirectionFromAngles(float yaw, float pitch)
{
Vector3 result;
result.X = (float)Math.Cos(yaw) * (float)Math.Cos(pitch);
result.Y = (float)Math.Sin(yaw) * (float)Math.Cos(pitch);
result.Z = (float)Math.Sin(pitch);
return result;
}
/// <summary>
/// Returns the angle between two 2D points. Note that the returned angle is
/// offset so that 0 degrees points directly down the +Y axis on a 2D cartesian
/// grid, instead of the +X axis as one might expect.
/// </summary>
/// <returns>the angle (in radians) between the two points</returns>
/// <param name="x1">X coordinate of the first point</param>
/// <param name="y1">Y coordinate of the first point</param>
/// <param name="x2">X coordinate of the second point</param>
/// <param name="y2">Y coordinate of the second point</param>
public static float GetAngleBetweenPoints(float x1, float y1, float x2, float y2)
{
float angle = (float)Math.Atan2(y2 - y1, x2 - x1);
// we offset the angle by 90 degrees to ensure 0 degrees points in the
// +Y direction on a 2D cartesian grid instead of +X. This corresponds with
// the rest of our direction coordinate system.
return angle - MathConstants.Radians90;
}
/// <summary>
/// Solves a quadratic equation and returns the lowest root.
/// </summary>
/// <returns>true if the quadratic could be solved, false if not</returns>
/// <param name="a">the value of the a variable in the quadratic equation</param>
/// <param name="b">the value of the b variable in the quadratic equation</param>
/// <param name="c">the value of the c variable in the quadratic equation</param>
/// <param name="maxR">the maximum root value we will accept as an answer. anything that is higher will not be accepted as a solution</param>
/// <param name="root">the variable to hold the lowest root value if one is found</param>
public static bool GetLowestQuadraticRoot(float a, float b, float c, float maxR, out float root)
{
root = 0.0f;
float determinant = (b * b) - (4.0f * a * c);
// If the determinant is negative, there is no solution (can't square root a negative)
if (determinant < 0.0f)
return false;
float sqrtDeterminant = (float)Math.Sqrt(determinant);
float root1 = (-b - sqrtDeterminant) / (2 * a);
float root2 = (-b + sqrtDeterminant) / (2 * a);
// Sort so root1 <= root2
if (root1 > root2)
{
float temp = root2;
root2 = root1;
root1 = temp;
}
// Get the lowest root
if (root1 > 0 && root1 < maxR)
{
root = root1;
return true;
}
if (root2 > 0 && root2 < maxR)
{
root = root2;
return true;
}
// No valid solutions found
return false;
}
/// <summary>
/// Returns the 2D point on a circle's circumference given a radius and angle.
/// </summary>
/// <param name="radius">the radius of the circle</param>
/// <param name="angle">the angle around the circle (in radians)</param>
/// <param name="x">the variable to hold the X point on the circle's circumference</param>
/// <param name="y">the variable to hold the Y point on the circle's circumference</param>
public static void GetPointOnCircle(float radius, float angle, out float x, out float y)
{
x = radius * (float)Math.Cos(angle);
y = radius * (float)Math.Sin(angle);
}
/// <summary>
/// Returns the 2D point on a circle's circumference given a radius and angle.
/// </summary>
/// <param name="radius">the radius of the circle</param>
/// <param name="angle">the angle around the circle (in radians)</param>
/// <param name="point">the vector to hold the point on the circle's circumference</param>
public static void GetPointOnCircle(float radius, float angle, out Vector2 point)
{
point.X = radius * (float)Math.Cos(angle);
point.Y = radius * (float)Math.Sin(angle);
}
/// <summary>
/// Clamps a value to a given range.
/// </summary>
/// <returns>the clamped value</returns>
/// <param name="value">the value to be clamped</param>
/// <param name="low">the low end of the range to clamp to</param>
/// <param name="high">the high end of the range to clamp to</param>
public static float Clamp(float value, float low, float high)
{
if (value < low)
return low;
if (value > high)
return high;
return value;
}
/// <summary>
/// Clamps a value to a given range.
/// </summary>
/// <returns>the clamped value</returns>
/// <param name="value">the value to be clamped</param>
/// <param name="low">the low end of the range to clamp to</param>
/// <param name="high">the high end of the range to clamp to</param>
public static int Clamp(int value, int low, int high)
{
if (value < low)
return low;
if (value > high)
return high;
return value;
}
/// <summary>
/// Converts degrees to radians.
/// </summary>
/// <returns>equivalent value in radians</returns>
/// <param name="degrees">degree value to convert</param>
public static float DegreesToRadians(float degrees)
{
return degrees * MathConstants.PiOver180;
}
/// <summary>
/// Checks two floats for equality using a defined "tolerance" to account
/// for floating point rounding errors, etc.
/// </summary>
/// <returns>true if equal, false if not</returns>
/// <param name="a">first value to check</param>
/// <param name="b">second value to check</param>
/// <param name="tolerance">tolerance value to use</param>
public static bool IsCloseEnough(float a, float b, float tolerance = MathConstants.Tolerance)
{
return Math.Abs((a - b) / ((b == 0.0f) ? 1.0f : b)) < tolerance;
}
/// <summary>
/// Determines if a given number is a power of two.
/// </summary>
/// <returns>true if a power of two, false if not</returns>
/// <param name="n">number to check</param>
public static bool IsPowerOf2(int n)
{
return (n != 0) && ((n & (n - 1)) == 0);
}
/// <summary>
/// Linearly interpolates between two values.
/// </summary>
/// <param name="a">first value (low end of range)</param>
/// <param name="b">second value (high end of range)</param>
/// <param name="t">the amount to interpolate between the two values</param>
public static float Lerp(float a, float b, float t)
{
return a + (b - a) * t;
}
/// <summary>
/// Given a linearly interpolated value and the original range (high and
/// low) of the linear interpolation, this will determine what the original
/// 0.0 to 1.0 value (weight) was used to perform the interpolation.
/// </summary>
/// <returns>the interpolation value (weight, in the range 0.0 to 1.0) used in the original interpolation</returns>
/// <param name="a">first value (low end of range)</param>
/// <param name="b">second value (high end of range)</param>
/// <param name="lerpValue">the result of the original interpolation</param>
public static float InverseLerp(float a, float b, float lerpValue)
{
return (lerpValue - a) / (b - a);
}
/// <summary>
/// Converts radians to degrees.
/// </summary>
/// <returns>equivalent value in degrees</returns>
/// <param name="radians">radian value to convert</param>
public static float RadiansToDegrees(float radians)
{
return radians * (1.0f / MathConstants.PiOver180);
}
/// <summary>
/// Clamps a value to a given range, but if the value is outside the range
/// instead of returning the low/high end of the range, this will continue
/// counting after moving to the opposite end of the range to arrive at a
/// final value.
/// </summary>
/// <returns>the clamped value</returns>
/// <param name="value">the value to be clamped</param>
/// <param name="low">the low end of the range to clamp to</param>
/// <param name="high">the high end of the range to clamp to</param>
public static float RolloverClamp(float value, float low, float high)
{
float temp = value;
// TODO: this is really shitty... make it better
do
{
float range = Math.Abs(high - low);
if (temp < low)
temp = temp + range;
if (value > high)
temp = temp - range;
}
while (temp < low || temp > high); // loop through as many times as necessary to put the value within the low/high range
return temp;
}
/// <summary>
/// Re-scales a given value from an old min/max range to a new and
/// different min/max range such that the value is approximately
/// at the same distance between both min and max values.
/// </summary>
/// <returns>re-scaled value which will fall between newMin and newMax</returns>
/// <param name="value">the value to be rescaled which is currently between originalMin and originalMax</param>
/// <param name="originalMin">original min value (low end of range)</param>
/// <param name="originalMax">original max value (high end of range)</param>
/// <param name="newMin">new min value (low end of range)</param>
/// <param name="newMax">new max value (high end of range)</param>
public static float ScaleRange(float value, float originalMin, float originalMax, float newMin, float newMax)
{
return (value / ((originalMax - originalMin) / (newMax - newMin))) + newMin;
}
/// <summary>
/// Interpolates between two values using a cubic equation.
/// </summary>
/// <returns>the interpolated value</returns>
/// <param name="low">low end of range to interpolate between</param>
/// <param name="high">high end of range to interpolate between</param>
/// <param name="t">amount to interpolate by (the weight)</param>
public static float SmoothStep(float low, float high, float t)
{
float n = Clamp(t, 0.0f, 1.0f);
return Lerp(low, high, (n * n) * (3.0f - (2.0f * n)));
}
}
}

668
Blarg.GameFramework/Math/Matrix3x3.cs Normal file
View file

@ -0,0 +1,668 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Matrix3x3 : IEquatable<Matrix3x3>
{
public float M11;
public float M21;
public float M31;
public float M12;
public float M22;
public float M32;
public float M13;
public float M23;
public float M33;
private const int _11 = 0;
private const int _12 = 3;
private const int _13 = 6;
private const int _21 = 1;
private const int _22 = 4;
private const int _23 = 7;
private const int _31 = 2;
private const int _32 = 5;
private const int _33 = 8;
public float Determinant
{
get
{
return
M11 * M22 * M33 +
M12 * M23 * M31 +
M13 * M21 * M32 -
M11 * M23 * M32 -
M12 * M21 * M33 -
M13 * M22 * M31;
}
}
public Vector3 Forward
{
get { return new Vector3(M13, M23, M33); }
}
public Vector3 Backward
{
get { return new Vector3(-M13, -M23, -M33); }
}
public Vector3 Left
{
get { return new Vector3(M11, M21, M31); }
}
public Vector3 Right
{
get { return new Vector3(-M11, -M21, -M31); }
}
public Vector3 Up
{
get { return new Vector3(M12, M22, M32); }
}
public Vector3 Down
{
get { return new Vector3(-M12, -M22, -M32); }
}
public Matrix3x3(
float m11, float m12, float m13,
float m21, float m22, float m23,
float m31, float m32, float m33
)
{
M11 = m11;
M12 = m12;
M13 = m13;
M21 = m21;
M22 = m22;
M23 = m23;
M31 = m31;
M32 = m32;
M33 = m33;
}
public Matrix3x3(float[] m)
{
M11 = m[_11];
M12 = m[_12];
M13 = m[_13];
M21 = m[_21];
M22 = m[_22];
M23 = m[_23];
M31 = m[_31];
M32 = m[_32];
M33 = m[_33];
}
public void Set(
float m11, float m12, float m13,
float m21, float m22, float m23,
float m31, float m32, float m33
)
{
M11 = m11;
M12 = m12;
M13 = m13;
M21 = m21;
M22 = m22;
M23 = m23;
M31 = m31;
M32 = m32;
M33 = m33;
}
public void Set(float[] m)
{
M11 = m[_11];
M12 = m[_12];
M13 = m[_13];
M21 = m[_21];
M22 = m[_22];
M23 = m[_23];
M31 = m[_31];
M32 = m[_32];
M33 = m[_33];
}
public static readonly Matrix3x3 Identity = new Matrix3x3(
1.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 1.0f
);
public Quaternion ToQuaternion()
{
Quaternion result;
ToQuaternion(out result);
return result;
}
public void ToQuaternion(out Quaternion result)
{
Quaternion temp;
float n = M11 + M22 + M33;
if (n > 0.0f)
{
float a = (float)Math.Sqrt(n + 1.0f);
temp.W = a / 2.0f;
a = 0.5f / a;
temp.X = (M32 - M23) * a;
temp.Y = (M13 - M31) * a;
temp.Z = (M21 - M12) * a;
}
else if ((M11 >= M22) && (M11 >= M33))
{
float a = (float)Math.Sqrt(1.0f + M11 - M22 - M33);
float b = 0.5f / a;
temp.X = 0.5f * a;
temp.Y = (M21 + M12) * b;
temp.Z = (M31 + M13) * b;
temp.W = (M32 - M23) * b;
}
else if (M22 > M33)
{
float a = (float)Math.Sqrt(1.0f + M22 - M11 - M33);
float b = 0.5f / a;
temp.X = (M12 + M21) * b;
temp.Y = 0.5f * a;
temp.Z = (M23 + M32) * b;
temp.W = (M13 - M31) * b;
}
else
{
float a = (float)Math.Sqrt(1.0f + M33 - M11 - M22);
float b = 0.5f / a;
temp.X = (M13 + M31) * b;
temp.Y = (M23 + M32) * b;
temp.Z = 0.5f * a;
temp.W = (M21 - M12) * b;
}
Quaternion.Normalize(ref temp, out result);
}
public Matrix4x4 ToMatrix4x4()
{
Matrix4x4 result;
ToMatrix4x4(out result);
return result;
}
public void ToMatrix4x4(out Matrix4x4 result)
{
result.M11 = M11;
result.M12 = M12;
result.M13 = M13;
result.M14 = 0.0f;
result.M21 = M21;
result.M22 = M22;
result.M23 = M23;
result.M24 = 0.0f;
result.M31 = M31;
result.M32 = M32;
result.M33 = M33;
result.M34 = 0.0f;
result.M41 = 0.0f;
result.M42 = 0.0f;
result.M43 = 0.0f;
result.M44 = 1.0f;
}
public void ToArray(float[] result)
{
if (result == null || result.Length < 9)
throw new ArgumentException();
result[0] = M11;
result[3] = M12;
result[6] = M13;
result[1] = M21;
result[4] = M22;
result[7] = M23;
result[2] = M31;
result[5] = M32;
result[8] = M33;
}
public float[] ToArray()
{
var result = new float[9];
ToArray(result);
return result;
}
public static Matrix3x3 CreateFromEulerAngles(float x, float y, float z)
{
Matrix3x3 result;
CreateFromEulerAngles(x, y, z, out result);
return result;
}
public static void CreateFromEulerAngles(float x, float y, float z, out Matrix3x3 result)
{
Matrix3x3 rotateZ;
Matrix3x3 rotateY;
Matrix3x3 rotateX;
Matrix3x3.CreateRotationZ(z, out rotateZ);
Matrix3x3.CreateRotationY(y, out rotateY);
Matrix3x3.CreateRotationX(x, out rotateX);
result = rotateZ * rotateY * rotateX;
}
public static Matrix3x3 CreateRotation(float angle, Vector3 axis)
{
Matrix3x3 result;
CreateRotation(angle, ref axis, out result);
return result;
}
public static void CreateRotation(float angle, ref Vector3 axis, out Matrix3x3 result)
{
float s = (float)Math.Sin(angle);
float c = (float)Math.Cos(angle);
result.M11 = (axis.X * axis.X) * (1.0f - c) + c;
result.M12 = (axis.X * axis.Y) * (1.0f - c) - (axis.Z * s);
result.M13 = (axis.X * axis.Z) * (1.0f - c) + (axis.Y * s);
result.M21 = (axis.Y * axis.X) * (1.0f - c) + (axis.Z * s);
result.M22 = (axis.Y * axis.Y) * (1.0f - c) + c;
result.M23 = (axis.Y * axis.Z) * (1.0f - c) - (axis.X * s);
result.M31 = (axis.Z * axis.X) * (1.0f - c) - (axis.Y * s);
result.M32 = (axis.Z * axis.Y) * (1.0f - c) + (axis.X * s);
result.M33 = (axis.Z * axis.Z) * (1.0f - c) + c;
}
public static Matrix3x3 CreateRotationX(float angle)
{
Matrix3x3 result;
CreateRotationX(angle, out result);
return result;
}
public static void CreateRotationX(float angle, out Matrix3x3 result)
{
float s = (float)Math.Sin(angle);
float c = (float)Math.Cos(angle);
result.M11 = 1.0f;
result.M12 = 0.0f;
result.M13 = 0.0f;
result.M21 = 0.0f;
result.M22 = c;
result.M23 = -s;
result.M31 = 0.0f;
result.M32 = s;
result.M33 = c;
}
public static Matrix3x3 CreateRotationY(float angle)
{
Matrix3x3 result;
CreateRotationY(angle, out result);
return result;
}
public static void CreateRotationY(float angle, out Matrix3x3 result)
{
float s = (float)Math.Sin(angle);
float c = (float)Math.Cos(angle);
result.M11 = c;
result.M12 = 0.0f;
result.M13 = s;
result.M21 = 0.0f;
result.M22 = 1.0f;
result.M23 = 0.0f;
result.M31 = -s;
result.M32 = 0.0f;
result.M33 = c;
}
public static Matrix3x3 CreateRotationZ(float angle)
{
Matrix3x3 result;
CreateRotationZ(angle, out result);
return result;
}
public static void CreateRotationZ(float angle, out Matrix3x3 result)
{
float s = (float)Math.Sin(angle);
float c = (float)Math.Cos(angle);
result.M11 = c;
result.M12 = -s;
result.M13 = 0.0f;
result.M21 = s;
result.M22 = c;
result.M23 = 0.0f;
result.M31 = 0.0f;
result.M32 = 0.0f;
result.M33 = 1.0f;
}
public static Matrix3x3 Inverse(Matrix3x3 m)
{
Matrix3x3 result;
Inverse(ref m, out result);
return result;
}
public static void Inverse(ref Matrix3x3 m, out Matrix3x3 result)
{
float d = m.Determinant;
if (MathHelpers.IsCloseEnough(d, 0.0f))
result = Matrix3x3.Identity;
else
{
Matrix3x3 temp;
d = 1.0f / d;
temp.M11 = d * (m.M22 * m.M33 - m.M32 * m.M23);
temp.M21 = d * (m.M31 * m.M23 - m.M21 * m.M33);
temp.M31 = d * (m.M21 * m.M32 - m.M31 * m.M22);
temp.M12 = d * (m.M32 * m.M13 - m.M12 * m.M33);
temp.M22 = d * (m.M11 * m.M33 - m.M31 * m.M13);
temp.M32 = d * (m.M31 * m.M12 - m.M11 * m.M32);
temp.M13 = d * (m.M12 * m.M23 - m.M22 * m.M13);
temp.M23 = d * (m.M21 * m.M13 - m.M11 * m.M23);
temp.M33 = d * (m.M11 * m.M22 - m.M21 * m.M12);
result = temp;
}
}
public static Matrix3x3 Transpose(Matrix3x3 m)
{
Matrix3x3 result;
Transpose(ref m, out result);
return result;
}
public static void Transpose(ref Matrix3x3 m, out Matrix3x3 result)
{
Matrix3x3 temp;
temp.M11 = m.M11;
temp.M12 = m.M21;
temp.M13 = m.M31;
temp.M21 = m.M12;
temp.M22 = m.M22;
temp.M23 = m.M32;
temp.M31 = m.M13;
temp.M32 = m.M23;
temp.M33 = m.M33;
result = temp;
}
public static Vector3 Transform(Matrix3x3 m, Vector3 v)
{
Vector3 result;
Transform(ref m, ref v, out result);
return result;
}
public static void Transform(ref Matrix3x3 m, ref Vector3 v, out Vector3 result)
{
Vector3 temp;
temp.X = v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13;
temp.Y = v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23;
temp.Z = v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33;
result = temp;
}
public static Matrix3x3 Add(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
{
result.M11 = left.M11 + right.M11;
result.M12 = left.M12 + right.M12;
result.M13 = left.M13 + right.M13;
result.M21 = left.M21 + right.M21;
result.M22 = left.M22 + right.M22;
result.M23 = left.M23 + right.M23;
result.M31 = left.M31 + right.M31;
result.M32 = left.M32 + right.M32;
result.M33 = left.M33 + right.M33;
}
public static Matrix3x3 Subtract(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
{
result.M11 = left.M11 - right.M11;
result.M12 = left.M12 - right.M12;
result.M13 = left.M13 - right.M13;
result.M21 = left.M21 - right.M21;
result.M22 = left.M22 - right.M22;
result.M23 = left.M23 - right.M23;
result.M31 = left.M31 - right.M31;
result.M32 = left.M32 - right.M32;
result.M33 = left.M33 - right.M33;
}
public static Matrix3x3 Multiply(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
{
Matrix3x3 m;
m.M11 = left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31;
m.M12 = left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32;
m.M13 = left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33;
m.M21 = left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31;
m.M22 = left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32;
m.M23 = left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33;
m.M31 = left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31;
m.M32 = left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32;
m.M33 = left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33;
result = m;
}
public static Matrix3x3 Multiply(Matrix3x3 left, float right)
{
Matrix3x3 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Matrix3x3 left, float right, out Matrix3x3 result)
{
result.M11 = left.M11 * right;
result.M12 = left.M12 * right;
result.M13 = left.M13 * right;
result.M21 = left.M21 * right;
result.M22 = left.M22 * right;
result.M23 = left.M23 * right;
result.M31 = left.M31 * right;
result.M32 = left.M32 * right;
result.M33 = left.M33 * right;
}
public static Matrix3x3 Divide(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Matrix3x3 left, ref Matrix3x3 right, out Matrix3x3 result)
{
result.M11 = left.M11 / right.M11;
result.M12 = left.M12 / right.M12;
result.M13 = left.M13 / right.M13;
result.M21 = left.M21 / right.M21;
result.M22 = left.M22 / right.M22;
result.M23 = left.M23 / right.M23;
result.M31 = left.M31 / right.M31;
result.M32 = left.M32 / right.M32;
result.M33 = left.M33 / right.M33;
}
public static Matrix3x3 Divide(Matrix3x3 left, float right)
{
Matrix3x3 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Matrix3x3 left, float right, out Matrix3x3 result)
{
result.M11 = left.M11 / right;
result.M12 = left.M12 / right;
result.M13 = left.M13 / right;
result.M21 = left.M21 / right;
result.M22 = left.M22 / right;
result.M23 = left.M23 / right;
result.M31 = left.M31 / right;
result.M32 = left.M32 / right;
result.M33 = left.M33 / right;
}
public static Matrix3x3 operator +(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Add(ref left, ref right, out result);
return result;
}
public static Matrix3x3 operator -(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Matrix3x3 operator *(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Matrix3x3 operator *(Matrix3x3 left, float right)
{
Matrix3x3 result;
Multiply(ref left, right, out result);
return result;
}
public static Matrix3x3 operator /(Matrix3x3 left, Matrix3x3 right)
{
Matrix3x3 result;
Divide(ref left, ref right, out result);
return result;
}
public static Matrix3x3 operator /(Matrix3x3 left, float right)
{
Matrix3x3 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Matrix3x3 left, Matrix3x3 right)
{
return left.Equals(right);
}
public static bool operator !=(Matrix3x3 left, Matrix3x3 right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is Matrix3x3)
return this.Equals((Matrix3x3)obj);
else
return false;
}
public bool Equals(Matrix3x3 other)
{
return (
M11 == other.M11 &&
M12 == other.M12 &&
M13 == other.M13 &&
M21 == other.M21 &&
M22 == other.M22 &&
M23 == other.M23 &&
M31 == other.M31 &&
M32 == other.M32 &&
M33 == other.M33
);
}
public override int GetHashCode()
{
return (
M11.GetHashCode() ^
M12.GetHashCode() ^
M13.GetHashCode() ^
M21.GetHashCode() ^
M22.GetHashCode() ^
M23.GetHashCode() ^
M31.GetHashCode() ^
M32.GetHashCode() ^
M33.GetHashCode()
);
}
public override string ToString()
{
return String.Format(
"{{{0}, {1}, {2}\n {3}, {4}, {5}\n {6}, {7}, {8}}}",
M11, M12, M13, M21, M22, M23, M31, M32, M33
);
}
}
}

1272
Blarg.GameFramework/Math/Matrix4x4.cs Normal file
View file

File diff suppressed because it is too large Load diff

155
Blarg.GameFramework/Math/Plane.cs Normal file
View file

@ -0,0 +1,155 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
public enum PlanePointClassify
{
InFront = 0,
Behind = 1,
OnPlane = 2
}
[StructLayout(LayoutKind.Sequential)]
public struct Plane : IEquatable<Plane>
{
public Vector3 Normal;
public float D;
public Plane(float a, float b, float c, float d)
{
Normal.X = a;
Normal.Y = b;
Normal.Z = c;
D = d;
}
public Plane(Vector3 origin, Vector3 normal)
: this(ref origin, ref normal)
{
}
public Plane(ref Vector3 origin, ref Vector3 normal)
{
Normal = normal;
D = -Vector3.Dot(ref origin, ref normal);
}
public Plane(Vector3 a, Vector3 b, Vector3 c)
: this(ref a, ref b, ref c)
{
}
public Plane(ref Vector3 a, ref Vector3 b, ref Vector3 c)
{
Vector3 e3 = b - a;
Vector3 e1 = c - b;
Vector3 crossed = Vector3.Cross(e3, e1);
float scaleFactor = 1.0f / crossed.Length;
Normal = crossed * scaleFactor;
D = -Vector3.Dot(a, Normal);
}
public static PlanePointClassify ClassifyPoint(Plane plane, Vector3 point)
{
return ClassifyPoint(ref plane, ref point);
}
public static PlanePointClassify ClassifyPoint(ref Plane plane, float x, float y, float z)
{
Vector3 point = new Vector3(x, y, z);
return ClassifyPoint(ref plane, ref point);
}
public static PlanePointClassify ClassifyPoint(ref Plane plane, ref Vector3 point)
{
float planeDot = Vector3.Dot(ref plane.Normal, ref point) + plane.D;
if (planeDot < 0.0f)
return PlanePointClassify.Behind;
else if (planeDot > 0.0f)
return PlanePointClassify.InFront;
else
return PlanePointClassify.OnPlane;
}
public static float DistanceBetween(Plane plane, Vector3 point)
{
return DistanceBetween(ref plane, ref point);
}
public static float DistanceBetween(ref Plane plane, ref Vector3 point)
{
return Vector3.Dot(ref point, ref plane.Normal) + plane.D;
}
public static bool IsFrontFacingTo(Plane plane, Vector3 direction)
{
return IsFrontFacingTo(ref plane, ref direction);
}
public static bool IsFrontFacingTo(ref Plane plane, ref Vector3 direction)
{
if (Vector3.Dot(plane.Normal, direction) <= 0.0f)
return true;
else
return false;
}
public static Plane Normalize(Plane plane)
{
Plane result;
Normalize(ref plane, out result);
return result;
}
public static void Normalize(ref Plane plane, out Plane result)
{
float length = (float)Math.Sqrt(
(plane.Normal.X * plane.Normal.X) +
(plane.Normal.Y * plane.Normal.Y) +
(plane.Normal.Z * plane.Normal.Z)
);
result.Normal.X = plane.Normal.X / length;
result.Normal.Y = plane.Normal.Y / length;
result.Normal.Z = plane.Normal.Z / length;
result.D = plane.D / length;
}
public static bool operator ==(Plane left, Plane right)
{
return left.Equals(right);
}
public static bool operator !=(Plane left, Plane right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is Plane)
return this.Equals((Plane)obj);
else
return false;
}
public bool Equals(Plane other)
{
return (Normal == other.Normal && D == other.D);
}
public override int GetHashCode()
{
return Normal.GetHashCode() ^ D.GetHashCode();
}
public override string ToString()
{
return String.Format("{{Normal:{0} D:{1}}}", Normal, D);
}
}
}

213
Blarg.GameFramework/Math/Point2.cs Normal file
View file

@ -0,0 +1,213 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Point2 : IEquatable<Point2>
{
public int X;
public int Y;
public static readonly Point2 Zero = new Point2(0, 0);
public Point2(int x, int y)
{
X = x;
Y = y;
}
public Point2(Point2 other)
: this(ref other)
{
}
public Point2(ref Point2 other)
{
X = other.X;
Y = other.Y;
}
public void Set(int x, int y)
{
X = x;
Y = y;
}
public void Set(Point2 other)
{
Set(ref other);
}
public void Set(ref Point2 other)
{
X = other.X;
Y = other.Y;
}
public static Point2 Add(Point2 left, Point2 right)
{
Point2 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Point2 left, ref Point2 right, out Point2 result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
}
public static Point2 Subtract(Point2 left, Point2 right)
{
Point2 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Point2 left, ref Point2 right, out Point2 result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
}
public static Point2 Multiply(Point2 left, Point2 right)
{
Point2 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Point2 left, ref Point2 right, out Point2 result)
{
result.X = left.X * right.X;
result.Y = left.Y * right.Y;
}
public static Point2 Multiply(Point2 left, float right)
{
Point2 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Point2 left, float right, out Point2 result)
{
result.X = (int)((float)left.X * right);
result.Y = (int)((float)left.Y * right);
}
public static Point2 Divide(Point2 left, Point2 right)
{
Point2 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Point2 left, ref Point2 right, out Point2 result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
}
public static Point2 Divide(Point2 left, float right)
{
Point2 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Point2 left, float right, out Point2 result)
{
result.X = (int)((float)left.X / right);
result.Y = (int)((float)left.Y / right);
}
public static Point2 operator +(Point2 left, Point2 right)
{
Point2 result;
Add(ref left, ref right, out result);
return result;
}
public static Point2 operator -(Point2 left, Point2 right)
{
Point2 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Point2 operator *(Point2 left, Point2 right)
{
Point2 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Point2 operator *(Point2 left, float right)
{
Point2 result;
Multiply(ref left, right, out result);
return result;
}
public static Point2 operator *(float left, Point2 right)
{
Point2 result;
Multiply(ref right, left, out result);
return result;
}
public static Point2 operator /(Point2 left, Point2 right)
{
Point2 result;
Divide(ref left, ref right, out result);
return result;
}
public static Point2 operator /(Point2 left, float right)
{
Point2 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Point2 left, Point2 right)
{
return left.Equals(right);
}
public static bool operator !=(Point2 left, Point2 right)
{
return !left.Equals(right);
}
public static Point2 operator -(Point2 left)
{
return new Point2(-left.X, -left.Y);
}
public override bool Equals(object obj)
{
if (obj is Point2)
return this.Equals((Point2)obj);
else
return false;
}
public bool Equals(Point2 other)
{
return (X == other.X && Y == other.Y);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1}}}", X, Y);
}
}
}

224
Blarg.GameFramework/Math/Point3.cs Normal file
View file

@ -0,0 +1,224 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Point3 : IEquatable<Point3>
{
public int X;
public int Y;
public int Z;
public static readonly Point3 Zero = new Point3(0, 0, 0);
public Point3(int x, int y, int z)
{
X = x;
Y = y;
Z = z;
}
public Point3(Point3 other)
: this(ref other)
{
}
public Point3(ref Point3 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
}
public void Set(int x, int y, int z)
{
X = x;
Y = y;
Z = z;
}
public void Set(Point3 other)
{
Set(ref other);
}
public void Set(ref Point3 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
}
public static Point3 Add(Point3 left, Point3 right)
{
Point3 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Point3 left, ref Point3 right, out Point3 result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
result.Z = left.Z + right.Z;
}
public static Point3 Subtract(Point3 left, Point3 right)
{
Point3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Point3 left, ref Point3 right, out Point3 result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
result.Z = left.Z - right.Z;
}
public static Point3 Multiply(Point3 left, Point3 right)
{
Point3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Point3 left, ref Point3 right, out Point3 result)
{
result.X = left.X * right.X;
result.Y = left.Y * right.Y;
result.Z = left.Z * right.Z;
}
public static Point3 Multiply(Point3 left, float right)
{
Point3 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Point3 left, float right, out Point3 result)
{
result.X = (int)((float)left.X * right);
result.Y = (int)((float)left.Y * right);
result.Z = (int)((float)left.Z * right);
}
public static Point3 Divide(Point3 left, Point3 right)
{
Point3 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Point3 left, ref Point3 right, out Point3 result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
result.Z = left.Z / right.Z;
}
public static Point3 Divide(Point3 left, float right)
{
Point3 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Point3 left, float right, out Point3 result)
{
result.X = (int)((float)left.X / right);
result.Y = (int)((float)left.Y / right);
result.Z = (int)((float)left.Z / right);
}
public static Point3 operator +(Point3 left, Point3 right)
{
Point3 result;
Add(ref left, ref right, out result);
return result;
}
public static Point3 operator -(Point3 left, Point3 right)
{
Point3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Point3 operator *(Point3 left, Point3 right)
{
Point3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Point3 operator *(Point3 left, float right)
{
Point3 result;
Multiply(ref left, right, out result);
return result;
}
public static Point3 operator *(float left, Point3 right)
{
Point3 result;
Multiply(ref right, left, out result);
return result;
}
public static Point3 operator /(Point3 left, Point3 right)
{
Point3 result;
Divide(ref left, ref right, out result);
return result;
}
public static Point3 operator /(Point3 left, float right)
{
Point3 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Point3 left, Point3 right)
{
return left.Equals(right);
}
public static bool operator !=(Point3 left, Point3 right)
{
return !left.Equals(right);
}
public static Point3 operator -(Point3 left)
{
return new Point3(-left.X, -left.Y, -left.Z);
}
public override bool Equals(object obj)
{
if (obj is Point3)
return this.Equals((Point3)obj);
else
return false;
}
public bool Equals(Point3 other)
{
return (X == other.X && Y == other.Y && Z == other.Z);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1} Z:{2}}}", X, Y, Z);
}
}
}

541
Blarg.GameFramework/Math/Quaternion.cs Normal file
View file

@ -0,0 +1,541 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion : IEquatable<Quaternion>
{
public float X;
public float Y;
public float Z;
public float W;
public float Length
{
get
{
return (float)Math.Sqrt(
(W * W) +
(X * X) +
(Y * Y) +
(Z * Z)
);
}
}
public float LengthSquared
{
get
{
return
(W * W) +
(X * X) +
(Y * Y) +
(Z * Z);
}
}
public Vector3 Vector
{
get { return new Vector3(X, Y, Z); }
}
public Quaternion(float x, float y, float z, float w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public Quaternion(Quaternion q)
: this(ref q)
{
}
public Quaternion(ref Quaternion q)
{
X = q.X;
Y = q.Y;
Z = q.Z;
W = q.W;
}
public void Set(float x, float y, float z, float w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public void Set(Quaternion q)
{
Set(ref q);
}
public void Set(ref Quaternion q)
{
X = q.X;
Y = q.Y;
Z = q.Z;
W = q.W;
}
public static readonly Quaternion Identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
public Matrix4x4 ToMatrix4x4()
{
Matrix4x4 result;
ToMatrix4x4(out result);
return result;
}
public void ToMatrix4x4(out Matrix4x4 result)
{
result.M11 = 1.0f - (2.0f * ((Y * Y) + (Z * Z)));
result.M21 = 2.0f * ((X * Y) + (Z * W));
result.M31 = 2.0f * ((Z * X) - (Y * W));
result.M41 = 0.0f;
result.M12 = 2.0f * ((X * Y) - (Z * W));
result.M22 = 1.0f - (2.0f * ((Z * Z) + (X * X)));
result.M32 = 2.0f * ((Y * Z) + (X * W));
result.M42 = 0.0f;
result.M13 = 2.0f * ((Z * X) + (Y * W));
result.M23 = 2.0f * ((Y * Z) - (X * W));
result.M33 = 1.0f - (2.0f * ((Y * Y) + (X * X)));
result.M43 = 0.0f;
result.M14 = 0.0f;
result.M24 = 0.0f;
result.M34 = 0.0f;
result.M44 = 1.0f;
}
public Matrix3x3 ToMatrix3x3()
{
Matrix3x3 result;
ToMatrix3x3(out result);
return result;
}
public void ToMatrix3x3(out Matrix3x3 result)
{
result.M11 = 1.0f - (2.0f * ((Y * Y) + (Z * Z)));
result.M21 = 2.0f * ((X * Y) + (Z * W));
result.M31 = 2.0f * ((Z * X) - (Y * W));
result.M12 = 2.0f * ((X * Y) - (Z * W));
result.M22 = 1.0f - (2.0f * ((Z * Z) + (X * X)));
result.M32 = 2.0f * ((Y * Z) + (X * W));
result.M13 = 2.0f * ((Z * X) + (Y * W));
result.M23 = 2.0f * ((Y * Z) - (X * W));
result.M33 = 1.0f - (2.0f * ((Y * Y) + (X * X)));
}
public static Quaternion Conjugate(Quaternion q)
{
Quaternion result;
Conjugate(ref q, out result);
return result;
}
public static void Conjugate(ref Quaternion q, out Quaternion result)
{
result.X = -q.X;
result.Y = -q.Y;
result.Z = -q.Z;
result.W = q.W;
}
public static Quaternion CreateFromAxisAngle(float angle, Vector3 axis)
{
Quaternion result;
CreateFromAxisAngle(angle, ref axis, out result);
return result;
}
public static void CreateFromAxisAngle(float angle, ref Vector3 axis, out Quaternion result)
{
float c = (float)Math.Cos(angle / 2.0f);
float s = (float)Math.Sin(angle / 2.0f);
result.W = c;
result.X = axis.X * s;
result.Y = axis.Y * s;
result.Z = axis.Z * s;
result = Normalize(result);
}
public static Quaternion CreateFromEulerAngles(float x, float y, float z)
{
Quaternion result;
CreateFromEulerAngles(x, y, z, out result);
return result;
}
public static void CreateFromEulerAngles(float x, float y, float z, out Quaternion result)
{
var qx = new Quaternion((float)Math.Sin(x / 2.0f), 0.0f, 0.0f, (float)Math.Cos(x / 2.0f));
var qy = new Quaternion(0.0f, (float)Math.Sin(y / 2.0f), 0.0f, (float)Math.Cos(y / 2.0f));
var qz = new Quaternion(0.0f, 0.0f, (float)Math.Sin(z / 2.0f), (float)Math.Cos(z / 2.0f));
result = Normalize(qz * qy * qx);
}
public static Quaternion Cross(Quaternion a, Quaternion b)
{
Quaternion result;
Cross(ref a, ref b, out result);
return result;
}
public static void Cross(ref Quaternion a, ref Quaternion b, out Quaternion result)
{
result = a * b;
}
public static float Dot(Quaternion a, Quaternion b)
{
return Dot(ref a, ref b);
}
public static float Dot(ref Quaternion a, ref Quaternion b)
{
return (
(a.W * b.W) +
(a.X * b.X) +
(a.Y * b.Y) +
(a.Z * b.Z)
);
}
public static void ExtractAxisAngle(Quaternion q, out float angle, out Vector3 axis)
{
ExtractAxisAngle(ref q, out angle, out axis);
}
public static void ExtractAxisAngle(ref Quaternion q, out float angle, out Vector3 axis)
{
angle = 2.0f * (float)Math.Acos(q.W);
float n = (float)Math.Sqrt(1.0f - (q.W * q.W));
if (n > 0.0001f)
axis = q.Vector / n;
else
axis = Vector3.XAxis;
}
public static Quaternion Inverse(Quaternion q)
{
Quaternion result;
Inverse(ref q, out result);
return result;
}
public static void Inverse(ref Quaternion q, out Quaternion result)
{
float inverseSquaredLength = 1.0f / q.LengthSquared;
result.X = -q.X * inverseSquaredLength;
result.Y = -q.Y * inverseSquaredLength;
result.Z = -q.Z * inverseSquaredLength;
result.W = q.W * inverseSquaredLength;
}
public static Quaternion Lerp(Quaternion a, Quaternion b, float interpolation)
{
Quaternion result;
Lerp(ref a, ref b, interpolation, out result);
return result;
}
public static void Lerp(ref Quaternion a, ref Quaternion b, float interpolation, out Quaternion result)
{
result = (a * (1.0f - interpolation)) + (b * interpolation);
}
public static Quaternion Normalize(Quaternion q)
{
Quaternion result;
Normalize(ref q, out result);
return result;
}
public static void Normalize(ref Quaternion q, out Quaternion result)
{
float inverseLength = 1.0f / q.Length;
result.X = q.X * inverseLength;
result.Y = q.Y * inverseLength;
result.Z = q.Z * inverseLength;
result.W = q.W * inverseLength;
}
public static Quaternion Slerp(Quaternion a, Quaternion b, float interpolation)
{
Quaternion result;
Slerp(ref a, ref b, interpolation, out result);
return result;
}
public static void Slerp(ref Quaternion a, ref Quaternion b, float interpolation, out Quaternion result)
{
if (a.LengthSquared == 0.0f)
{
if (b.LengthSquared == 0.0f)
{
result = Identity;
return;
}
else
{
result = b;
return;
}
}
else if (b.LengthSquared == 0.0f)
{
result = a;
return;
}
Quaternion q1 = a;
Quaternion q2 = b;
float cosHalfAngle = q1.W * q2.W + Vector3.Dot(q1.Vector, q2.Vector);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
result = q1;
return;
}
else if (cosHalfAngle < 0.0f)
{
q2.X = -q2.X;
q2.Y = -q2.Y;
q2.Z = -q2.Z;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
float halfAngle = (float)Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Math.Sin(halfAngle * (1.0f - interpolation)) * oneOverSinHalfAngle;
blendB = (float)Math.Sin(halfAngle * interpolation) * oneOverSinHalfAngle;
}
else
{
blendA = 1.0f - interpolation;
blendB = interpolation;
}
var v = q1.Vector * blendA + q2.Vector * blendB;
float w = q1.W * blendA + q2.W * blendB;
var temp = new Quaternion(v.X, v.Y, v.Z, w);
if (temp.LengthSquared > 0.0f)
result = Normalize(temp);
else
result = Identity;
}
public static Vector3 Transform(Quaternion q, Vector3 v)
{
Vector3 result;
Transform(ref q, ref v, out result);
return result;
}
public static void Transform(ref Quaternion q, ref Vector3 v, out Vector3 result)
{
Vector3 temp;
temp.X = ((v.X * ((1.0f - (q.Y * (q.Y + q.Y))) - (q.Z * (q.Z + q.Z)))) + (v.Y * ((q.X * (q.Y + q.Y)) - (q.W * (q.Z + q.Z))))) + (v.Z * ((q.X * (q.Z + q.Z)) + (q.W * (q.Y + q.Y))));
temp.Y = ((v.X * ((q.X * (q.Y + q.Y)) + (q.W * (q.Z + q.Z)))) + (v.Y * ((1.0f - (q.X * (q.X + q.X))) - (q.Z * (q.Z + q.Z))))) + (v.Z * ((q.Y * (q.Z + q.Z)) - (q.W * (q.X + q.X))));
temp.Z = ((v.X * ((q.X * (q.Z + q.Z)) - (q.W * (q.Y + q.Y)))) + (v.Y * ((q.Y * (q.Z + q.Z)) + (q.W * (q.X + q.X))))) + (v.Z * ((1.0f - (q.X * (q.X + q.X))) - (q.Y * (q.Y + q.Y))));
result = temp;
}
public static Quaternion Add(Quaternion left, Quaternion right)
{
Quaternion result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
result.Z = left.Z + right.Z;
result.W = left.W + right.W;
}
public static Quaternion Subtract(Quaternion left, Quaternion right)
{
Quaternion result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
result.Z = left.Z - right.Z;
result.W = left.W - right.W;
}
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
Quaternion result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
float X = (left.W * right.X) + (left.X * right.W) + (left.Y * right.Z) - (left.Z * right.Y);
float Y = (left.W * right.Y) + (left.Y * right.W) + (left.Z * right.X) - (left.X * right.Z);
float Z = (left.W * right.Z) + (left.Z * right.W) + (left.X * right.Y) - (left.Y * right.X);
float W = (left.W * right.W) - (left.X * right.X) - (left.Y * right.Y) - (left.Z * right.Z);
result.X = X;
result.Y = Y;
result.Z = Z;
result.W = W;
}
public static Quaternion Multiply(Quaternion left, float right)
{
Quaternion result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Quaternion left, float right, out Quaternion result)
{
result.X = left.X * right;
result.Y = left.Y * right;
result.Z = left.Z * right;
result.W = left.W * right;
}
public static Quaternion Divide(Quaternion left, Quaternion right)
{
Quaternion result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
result.Z = left.Z / right.Z;
result.W = left.W / right.W;
}
public static Quaternion Divide(Quaternion left, float right)
{
Quaternion result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Quaternion left, float right, out Quaternion result)
{
result.X = left.X / right;
result.Y = left.Y / right;
result.Z = left.Z / right;
result.W = left.W / right;
}
public static Quaternion operator +(Quaternion left, Quaternion right)
{
Quaternion result;
Add(ref left, ref right, out result);
return result;
}
public static Quaternion operator -(Quaternion left, Quaternion right)
{
Quaternion result;
Subtract(ref left, ref right, out result);
return result;
}
public static Quaternion operator *(Quaternion left, Quaternion right)
{
Quaternion result;
Multiply(ref left, ref right, out result);
return result;
}
public static Quaternion operator *(Quaternion left, float right)
{
Quaternion result;
Multiply(ref left, right, out result);
return result;
}
public static Quaternion operator /(Quaternion left, Quaternion right)
{
Quaternion result;
Divide(ref left, ref right, out result);
return result;
}
public static Quaternion operator /(Quaternion left, float right)
{
Quaternion result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Quaternion left, Quaternion right)
{
return left.Equals(right);
}
public static bool operator !=(Quaternion left, Quaternion right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is Quaternion)
return this.Equals((Quaternion)obj);
else
return false;
}
public bool Equals(Quaternion other)
{
return (
X == other.X &&
Y == other.Y &&
Z == other.Z &&
W == other.W
);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W);
}
}
}

80
Blarg.GameFramework/Math/Ray.cs Normal file
View file

@ -0,0 +1,80 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Ray : IEquatable<Ray>
{
public Vector3 Position;
public Vector3 Direction;
public Ray(Vector3 position, Vector3 direction)
: this(ref position, ref direction)
{
}
public Ray(ref Vector3 position, ref Vector3 direction)
{
Position = position;
Direction = direction;
}
public Ray(float positionX, float positionY, float positionZ, float directionX, float directionY, float directionZ)
{
Position.X = positionX;
Position.Y = positionY;
Position.Z = positionZ;
Direction.X = directionX;
Direction.Y = directionY;
Direction.Z = directionZ;
}
public Vector3 GetPositionAt(float distance)
{
Vector3 result;
GetPositionAt(distance, out result);
return result;
}
public void GetPositionAt(float distance, out Vector3 result)
{
result.X = Direction.X * distance + Position.X;
result.Y = Direction.Y * distance + Position.Y;
result.Z = Direction.Z * distance + Position.Z;
}
public static bool operator ==(Ray left, Ray right)
{
return left.Equals(right);
}
public static bool operator !=(Ray left, Ray right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is Ray)
return this.Equals((Ray)obj);
else
return false;
}
public bool Equals(Ray other)
{
return (Position == other.Position && Direction == other.Direction);
}
public override int GetHashCode()
{
return Position.GetHashCode() ^ Direction.GetHashCode();
}
public override string ToString()
{
return String.Format("{{Origin:{0} Direction:{1}}}", Position, Direction);
}
}
}

113
Blarg.GameFramework/Math/RectF.cs Normal file
View file

@ -0,0 +1,113 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct RectF : IEquatable<RectF>
{
public float Left;
public float Top;
public float Right;
public float Bottom;
public float Width
{
get { return Math.Abs(Right - Left); }
}
public float Height
{
get { return Math.Abs(Bottom - Top); }
}
public RectF(float left, float top, float right, float bottom)
{
Left = left;
Top = top;
Right = right;
Bottom = bottom;
}
public RectF(ref RectF r)
{
Left = r.Left;
Top = r.Top;
Right = r.Right;
Bottom = r.Bottom;
}
public RectF(RectF r)
{
Left = r.Left;
Top = r.Top;
Right = r.Right;
Bottom = r.Bottom;
}
public void Set(float left, float top, float right, float bottom)
{
Left = left;
Top = top;
Right = right;
Bottom = bottom;
}
public void Set(ref RectF r)
{
Left = r.Left;
Top = r.Top;
Right = r.Right;
Bottom = r.Bottom;
}
public void Set(RectF r)
{
Left = r.Left;
Top = r.Top;
Right = r.Right;
Bottom = r.Bottom;
}
public bool Contains(float x, float y)
{
if (x >= Left && y >= Top && x <= Right && y <= Bottom)
return true;
else
return false;
}
public static bool operator ==(RectF left, RectF right)
{
return left.Equals(right);
}
public static bool operator !=(RectF left, RectF right)
{
return !left.Equals(right);
}
public override bool Equals(object obj)
{
if (obj is RectF)
return this.Equals((RectF)obj);
else
return false;
}
public bool Equals(RectF other)
{
return (Left == other.Left && Top == other.Top && Right == other.Right && Bottom == other.Bottom);
}
public override int GetHashCode()
{
return Left.GetHashCode() ^ Top.GetHashCode() ^ Right.GetHashCode() ^ Bottom.GetHashCode();
}
public override string ToString()
{
return String.Format("{{{0}-{1},{2}-{3}}}", Left, Top, Right, Bottom);
}
}
}

21
Blarg.GameFramework/Math/SweptEllipsoidCollisionPacket.cs Normal file
View file

@ -0,0 +1,21 @@
using System;
namespace Blarg.GameFramework
{
public struct SweptEllipsoidCollisionPacket
{
// defines the x/y/z radius of the entity being checked
public Vector3 EllipsoidRadius;
public bool FoundCollision;
public float NearestDistance;
// the below fields are all in "ellipsoid space"
public Vector3 esVelocity; // velocity of the entity
public Vector3 esNormalizedVelocity;
public Vector3 esPosition; // current position of the entity
public Vector3 esIntersectionPoint; // if an intersection is found ...
}
}

64
Blarg.GameFramework/Math/Transformation.cs Normal file
View file

@ -0,0 +1,64 @@
using System;
namespace Blarg.GameFramework
{
public class Transformation
{
public Matrix4x4 Transform = Matrix4x4.Identity;
public void Rotate(float radians, float axisX, float axisY, float axisZ)
{
var axis = new Vector3(axisX, axisY, axisZ);
Matrix4x4 rotation;
Matrix4x4.CreateRotation(radians, ref axis, out rotation);
Transform *= rotation;
}
public void Rotate(float radians, ref Vector3 axis)
{
Matrix4x4 rotation;
Matrix4x4.CreateRotation(radians, ref axis, out rotation);
Transform *= rotation;
}
public void Scale(float scaleFactor)
{
Matrix4x4 scale;
Matrix4x4.CreateScale(scaleFactor, scaleFactor, scaleFactor, out scale);
Transform *= scale;
}
public void Scale(float x, float y, float z)
{
Matrix4x4 scale;
Matrix4x4.CreateScale(x, y, z, out scale);
Transform *= scale;
}
public void Scale(ref Vector3 v)
{
Matrix4x4 scale;
Matrix4x4.CreateScale(v.X, v.Y, v.Z, out scale);
Transform *= scale;
}
public void Translate(float x, float y, float z)
{
Matrix4x4 translation;
Matrix4x4.CreateTranslation(x, y, z, out translation);
Transform *= translation;
}
public void Translate(ref Vector3 v)
{
Matrix4x4 translation;
Matrix4x4.CreateTranslation(v.X, v.Y, v.Z, out translation);
Transform *= translation;
}
public void Reset()
{
Transform = Matrix4x4.Identity;
}
}
}

337
Blarg.GameFramework/Math/Vector2.cs Normal file
View file

@ -0,0 +1,337 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Vector2 : IEquatable<Vector2>
{
public float X;
public float Y;
public static readonly Vector2 Zero = new Vector2(0.0f, 0.0f);
public static readonly Vector2 XAxis = new Vector2(1.0f, 0.0f);
public static readonly Vector2 YAxis = new Vector2(0.0f, 1.0f);
public float Length
{
get
{
return (float)Math.Sqrt(
(X * X) +
(Y * Y)
);
}
}
public float LengthSquared
{
get
{
return
(X * X) +
(Y * Y);
}
}
public Vector2(float x, float y)
{
X = x;
Y = y;
}
public Vector2(Vector2 other)
: this(ref other)
{
}
public Vector2(ref Vector2 other)
{
X = other.X;
Y = other.Y;
}
public Vector2(Point2 point)
: this(ref point)
{
}
public Vector2(ref Point2 point)
{
X = (float)point.X;
Y = (float)point.Y;
}
public void Set(float x, float y)
{
X = x;
Y = y;
}
public void Set(Vector2 other)
{
Set(ref other);
}
public void Set(ref Vector2 other)
{
X = other.X;
Y = other.Y;
}
public void Set(Point2 point)
{
Set(ref point);
}
public void Set(ref Point2 point)
{
X = (float)point.X;
Y = (float)point.Y;
}
public static float Distance(Vector2 a, Vector2 b)
{
return Distance(ref a, ref b);
}
public static float Distance(ref Vector2 a, ref Vector2 b)
{
return (float)Math.Sqrt(
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y))
);
}
public static float DistanceSquared(Vector2 a, Vector2 b)
{
return DistanceSquared(ref a, ref b);
}
public static float DistanceSquared(ref Vector2 a, ref Vector2 b)
{
return
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y));
}
public static float Dot(Vector2 a, Vector2 b)
{
return Dot(ref a, ref b);
}
public static float Dot(ref Vector2 a, ref Vector2 b)
{
return
(a.X * b.X) +
(a.Y * b.Y);
}
public static Vector2 Lerp(Vector2 a, Vector2 b, float interpolation)
{
Vector2 result;
Lerp(ref a, ref b, interpolation, out result);
return result;
}
public static void Lerp(ref Vector2 a, ref Vector2 b, float interpolation, out Vector2 result)
{
result.X = a.X + (b.X - a.X) * interpolation;
result.Y = a.Y + (b.Y - a.Y) * interpolation;
}
public static Vector2 Normalize(Vector2 v)
{
Vector2 result;
Normalize(ref v, out result);
return result;
}
public static void Normalize(ref Vector2 v, out Vector2 result)
{
float inverseLength = 1.0f / v.Length;
result.X = v.X * inverseLength;
result.Y = v.Y * inverseLength;
}
public static Vector2 SetLength(Vector2 v, float length)
{
Vector2 result;
SetLength(ref v, length, out result);
return result;
}
public static void SetLength(ref Vector2 v, float length, out Vector2 result)
{
float temp = length / v.Length;
result.X = v.X * temp;
result.Y = v.Y * temp;
}
public static Vector2 Add(Vector2 left, Vector2 right)
{
Vector2 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Vector2 left, ref Vector2 right, out Vector2 result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
}
public static Vector2 Subtract(Vector2 left, Vector2 right)
{
Vector2 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Vector2 left, ref Vector2 right, out Vector2 result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
}
public static Vector2 Multiply(Vector2 left, Vector2 right)
{
Vector2 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Vector2 left, ref Vector2 right, out Vector2 result)
{
result.X = left.X * right.X;
result.Y = left.Y * right.Y;
}
public static Vector2 Multiply(Vector2 left, float right)
{
Vector2 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Vector2 left, float right, out Vector2 result)
{
result.X = left.X * right;
result.Y = left.Y * right;
}
public static Vector2 Divide(Vector2 left, Vector2 right)
{
Vector2 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Vector2 left, ref Vector2 right, out Vector2 result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
}
public static Vector2 Divide(Vector2 left, float right)
{
Vector2 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Vector2 left, float right, out Vector2 result)
{
result.X = left.X / right;
result.Y = left.Y / right;
}
public static Vector2 operator +(Vector2 left, Vector2 right)
{
Vector2 result;
Add(ref left, ref right, out result);
return result;
}
public static Vector2 operator -(Vector2 left, Vector2 right)
{
Vector2 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Vector2 operator *(Vector2 left, Vector2 right)
{
Vector2 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Vector2 operator *(Vector2 left, float right)
{
Vector2 result;
Multiply(ref left, right, out result);
return result;
}
public static Vector2 operator *(float left, Vector2 right)
{
Vector2 result;
Multiply(ref right, left, out result);
return result;
}
public static Vector2 operator /(Vector2 left, Vector2 right)
{
Vector2 result;
Divide(ref left, ref right, out result);
return result;
}
public static Vector2 operator /(Vector2 left, float right)
{
Vector2 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Vector2 left, Vector2 right)
{
return left.Equals(right);
}
public static bool operator !=(Vector2 left, Vector2 right)
{
return !left.Equals(right);
}
public static Vector2 operator -(Vector2 left)
{
return new Vector2(-left.X, -left.Y);
}
public override bool Equals(object obj)
{
if (obj is Vector2)
return this.Equals((Vector2)obj);
else
return false;
}
public bool Equals(Vector2 other)
{
return (X == other.X && Y == other.Y);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1}}}", X, Y);
}
}
}

391
Blarg.GameFramework/Math/Vector3.cs Normal file
View file

@ -0,0 +1,391 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Vector3 : IEquatable<Vector3>
{
public float X;
public float Y;
public float Z;
public static readonly Vector3 Zero = new Vector3(0.0f, 0.0f, 0.0f);
public static readonly Vector3 XAxis = new Vector3(1.0f, 0.0f, 0.0f);
public static readonly Vector3 YAxis = new Vector3(0.0f, 1.0f, 0.0f);
public static readonly Vector3 ZAxis = new Vector3(0.0f, 0.0f, 1.0f);
public static readonly Vector3 Up = new Vector3(0.0f, 1.0f, 0.0f);
public static readonly Vector3 Down = new Vector3(0.0f, -1.0f, 0.0f);
public static readonly Vector3 Forward = new Vector3(0.0f, 0.0f, -1.0f);
public static readonly Vector3 Backward = new Vector3(0.0f, 0.0f, 1.0f);
public static readonly Vector3 Left = new Vector3(-1.0f, 0.0f, 0.0f);
public static readonly Vector3 Right = new Vector3(1.0f, 0.0f, 0.0f);
public float Length
{
get
{
return (float)Math.Sqrt(
(X * X) +
(Y * Y) +
(Z * Z)
);
}
}
public float LengthSquared
{
get
{
return
(X * X) +
(Y * Y) +
(Z * Z);
}
}
public Vector3(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public Vector3(Vector3 other)
: this(ref other)
{
}
public Vector3(ref Vector3 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
}
public Vector3(Point3 point)
: this(ref point)
{
}
public Vector3(ref Point3 point)
{
X = (float)point.X;
Y = (float)point.Y;
Z = (float)point.Z;
}
public void Set(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public void Set(Vector3 other)
{
Set(ref other);
}
public void Set(ref Vector3 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
}
public void Set(Point3 point)
{
Set(ref point);
}
public void Set(ref Point3 point)
{
X = (float)point.X;
Y = (float)point.Y;
Z = (float)point.Z;
}
public static Vector3 Cross(Vector3 a, Vector3 b)
{
Vector3 result;
Cross(ref a, ref b, out result);
return result;
}
public static void Cross(ref Vector3 a, ref Vector3 b, out Vector3 result)
{
result.X = (a.Y * b.Z) - (a.Z * b.Y);
result.Y = (a.Z * b.X) - (a.X * b.Z);
result.Z = (a.X * b.Y) - (a.Y * b.X);
}
public static float Distance(Vector3 a, Vector3 b)
{
return Distance(ref a, ref b);
}
public static float Distance(ref Vector3 a, ref Vector3 b)
{
return (float)Math.Sqrt(
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y)) +
((b.Z - a.Z) * (b.Z - a.Z))
);
}
public static float DistanceSquared(Vector3 a, Vector3 b)
{
return DistanceSquared(ref a, ref b);
}
public static float DistanceSquared(ref Vector3 a, ref Vector3 b)
{
return
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y)) +
((b.Z - a.Z) * (b.Z - a.Z));
}
public static float Dot(Vector3 a, Vector3 b)
{
return Dot(ref a, ref b);
}
public static float Dot(ref Vector3 a, ref Vector3 b)
{
return
(a.X * b.X) +
(a.Y * b.Y) +
(a.Z * b.Z);
}
public static Vector3 Lerp(Vector3 a, Vector3 b, float interpolation)
{
Vector3 result;
Lerp(ref a, ref b, interpolation, out result);
return result;
}
public static void Lerp(ref Vector3 a, ref Vector3 b, float interpolation, out Vector3 result)
{
result.X = a.X + (b.X - a.X) * interpolation;
result.Y = a.Y + (b.Y - a.Y) * interpolation;
result.Z = a.Z + (b.Z - a.Z) * interpolation;
}
public static Vector3 Normalize(Vector3 v)
{
Vector3 result;
Normalize(ref v, out result);
return result;
}
public static void Normalize(ref Vector3 v, out Vector3 result)
{
float inverseLength = 1.0f / v.Length;
result.X = v.X * inverseLength;
result.Y = v.Y * inverseLength;
result.Z = v.Z * inverseLength;
}
public static Vector3 SetLength(Vector3 v, float length)
{
Vector3 result;
SetLength(ref v, length, out result);
return result;
}
public static void SetLength(ref Vector3 v, float length, out Vector3 result)
{
float temp = length / v.Length;
result.X = v.X * temp;
result.Y = v.Y * temp;
result.Z = v.Z * temp;
}
public static Vector3 SurfaceNormal(Vector3 a, Vector3 b, Vector3 c)
{
Vector3 result;
SurfaceNormal(ref a, ref b, ref c, out result);
return result;
}
public static void SurfaceNormal(ref Vector3 a, ref Vector3 b, ref Vector3 c, out Vector3 result)
{
result = Normalize(Cross((b - a), (c - a)));
}
public static Vector3 Add(Vector3 left, Vector3 right)
{
Vector3 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Vector3 left, ref Vector3 right, out Vector3 result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
result.Z = left.Z + right.Z;
}
public static Vector3 Subtract(Vector3 left, Vector3 right)
{
Vector3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Vector3 left, ref Vector3 right, out Vector3 result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
result.Z = left.Z - right.Z;
}
public static Vector3 Multiply(Vector3 left, Vector3 right)
{
Vector3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Vector3 left, ref Vector3 right, out Vector3 result)
{
result.X = left.X * right.X;
result.Y = left.Y * right.Y;
result.Z = left.Z * right.Z;
}
public static Vector3 Multiply(Vector3 left, float right)
{
Vector3 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Vector3 left, float right, out Vector3 result)
{
result.X = left.X * right;
result.Y = left.Y * right;
result.Z = left.Z * right;
}
public static Vector3 Divide(Vector3 left, Vector3 right)
{
Vector3 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Vector3 left, ref Vector3 right, out Vector3 result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
result.Z = left.Z / right.Z;
}
public static Vector3 Divide(Vector3 left, float right)
{
Vector3 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Vector3 left, float right, out Vector3 result)
{
result.X = left.X / right;
result.Y = left.Y / right;
result.Z = left.Z / right;
}
public static Vector3 operator +(Vector3 left, Vector3 right)
{
Vector3 result;
Add(ref left, ref right, out result);
return result;
}
public static Vector3 operator -(Vector3 left, Vector3 right)
{
Vector3 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Vector3 operator *(Vector3 left, Vector3 right)
{
Vector3 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Vector3 operator *(Vector3 left, float right)
{
Vector3 result;
Multiply(ref left, right, out result);
return result;
}
public static Vector3 operator *(float left, Vector3 right)
{
Vector3 result;
Multiply(ref right, left, out result);
return result;
}
public static Vector3 operator /(Vector3 left, Vector3 right)
{
Vector3 result;
Divide(ref left, ref right, out result);
return result;
}
public static Vector3 operator /(Vector3 left, float right)
{
Vector3 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Vector3 left, Vector3 right)
{
return left.Equals(right);
}
public static bool operator !=(Vector3 left, Vector3 right)
{
return !left.Equals(right);
}
public static Vector3 operator -(Vector3 left)
{
return new Vector3(-left.X, -left.Y, -left.Z);
}
public override bool Equals(object obj)
{
if (obj is Vector3)
return this.Equals((Vector3)obj);
else
return false;
}
public bool Equals(Vector3 other)
{
return (X == other.X && Y == other.Y && Z == other.Z);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1} Z:{2}}}", X, Y, Z);
}
}
}

351
Blarg.GameFramework/Math/Vector4.cs Normal file
View file

@ -0,0 +1,351 @@
using System;
using System.Runtime.InteropServices;
namespace Blarg.GameFramework
{
[StructLayout(LayoutKind.Sequential)]
public struct Vector4 : IEquatable<Vector4>
{
public float X;
public float Y;
public float Z;
public float W;
public static readonly Vector4 Zero = new Vector4(0.0f, 0.0f, 0.0f, 0.0f);
public float Length
{
get
{
return (float)Math.Sqrt(
(X * X) +
(Y * Y) +
(Z * Z) +
(W * W)
);
}
}
public float LengthSquared
{
get
{
return
(X * X) +
(Y * Y) +
(Z * Z) +
(W * W);
}
}
public Vector4(float x, float y, float z, float w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public Vector4(Vector4 other)
: this(ref other)
{
}
public Vector4(ref Vector4 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
W = other.W;
}
public void Set(float x, float y, float z, float w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public void Set(Vector4 other)
{
Set(ref other);
}
public void Set(ref Vector4 other)
{
X = other.X;
Y = other.Y;
Z = other.Z;
W = other.W;
}
public static float Distance(Vector4 a, Vector4 b)
{
return Distance(ref a, ref b);
}
public static float Distance(ref Vector4 a, ref Vector4 b)
{
return (float)Math.Sqrt(
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y)) +
((b.Z - a.Z) * (b.Z - a.Z)) +
((b.W - a.W) * (b.W - a.W))
);
}
public static float DistanceSquared(Vector4 a, Vector4 b)
{
return DistanceSquared(ref a, ref b);
}
public static float DistanceSquared(ref Vector4 a, ref Vector4 b)
{
return
((b.X - a.X) * (b.X - a.X)) +
((b.Y - a.Y) * (b.Y - a.Y)) +
((b.Z - a.Z) * (b.Z - a.Z)) +
((b.W - a.W) * (b.W - a.W));
}
public static float Dot(Vector4 a, Vector4 b)
{
return Dot(ref a, ref b);
}
public static float Dot(ref Vector4 a, ref Vector4 b)
{
return
(a.X * b.X) +
(a.Y * b.Y) +
(a.Z * b.Z) +
(a.W * b.W);
}
public static Vector4 Lerp(Vector4 a, Vector4 b, float interpolation)
{
Vector4 result;
Lerp(ref a, ref b, interpolation, out result);
return result;
}
public static void Lerp(ref Vector4 a, ref Vector4 b, float interpolation, out Vector4 result)
{
result.X = a.X + (b.X - a.X) * interpolation;
result.Y = a.Y + (b.Y - a.Y) * interpolation;
result.Z = a.Z + (b.Z - a.Z) * interpolation;
result.W = a.W + (b.W - a.W) * interpolation;
}
public static Vector4 Normalize(Vector4 v)
{
Vector4 result;
Normalize(ref v, out result);
return result;
}
public static void Normalize(ref Vector4 v, out Vector4 result)
{
float inverseLength = 1.0f / v.Length;
result.X = v.X * inverseLength;
result.Y = v.Y * inverseLength;
result.Z = v.Z * inverseLength;
result.W = v.W * inverseLength;
}
public static Vector4 SetLength(Vector4 v, float length)
{
Vector4 result;
SetLength(ref v, length, out result);
return result;
}
public static void SetLength(ref Vector4 v, float length, out Vector4 result)
{
float temp = length / v.Length;
result.X = v.X * temp;
result.Y = v.Y * temp;
result.Z = v.Z * temp;
result.W = v.W * temp;
}
public static Vector4 Add(Vector4 left, Vector4 right)
{
Vector4 result;
Add(ref left, ref right, out result);
return result;
}
public static void Add(ref Vector4 left, ref Vector4 right, out Vector4 result)
{
result.X = left.X + right.X;
result.Y = left.Y + right.Y;
result.Z = left.Z + right.Z;
result.W = left.W + right.W;
}
public static Vector4 Subtract(Vector4 left, Vector4 right)
{
Vector4 result;
Subtract(ref left, ref right, out result);
return result;
}
public static void Subtract(ref Vector4 left, ref Vector4 right, out Vector4 result)
{
result.X = left.X - right.X;
result.Y = left.Y - right.Y;
result.Z = left.Z - right.Z;
result.W = left.W - right.W;
}
public static Vector4 Multiply(Vector4 left, Vector4 right)
{
Vector4 result;
Multiply(ref left, ref right, out result);
return result;
}
public static void Multiply(ref Vector4 left, ref Vector4 right, out Vector4 result)
{
result.X = left.X * right.X;
result.Y = left.Y * right.Y;
result.Z = left.Z * right.Z;
result.W = left.W * right.W;
}
public static Vector4 Multiply(Vector4 left, float right)
{
Vector4 result;
Multiply(ref left, right, out result);
return result;
}
public static void Multiply(ref Vector4 left, float right, out Vector4 result)
{
result.X = left.X * right;
result.Y = left.Y * right;
result.Z = left.Z * right;
result.W = left.W * right;
}
public static Vector4 Divide(Vector4 left, Vector4 right)
{
Vector4 result;
Divide(ref left, ref right, out result);
return result;
}
public static void Divide(ref Vector4 left, ref Vector4 right, out Vector4 result)
{
result.X = left.X / right.X;
result.Y = left.Y / right.Y;
result.Z = left.Z / right.Z;
result.W = left.W / right.W;
}
public static Vector4 Divide(Vector4 left, float right)
{
Vector4 result;
Divide(ref left, right, out result);
return result;
}
public static void Divide(ref Vector4 left, float right, out Vector4 result)
{
result.X = left.X / right;
result.Y = left.Y / right;
result.Z = left.Z / right;
result.W = left.W / right;
}
public static Vector4 operator +(Vector4 left, Vector4 right)
{
Vector4 result;
Add(ref left, ref right, out result);
return result;
}
public static Vector4 operator -(Vector4 left, Vector4 right)
{
Vector4 result;
Subtract(ref left, ref right, out result);
return result;
}
public static Vector4 operator *(Vector4 left, Vector4 right)
{
Vector4 result;
Multiply(ref left, ref right, out result);
return result;
}
public static Vector4 operator *(Vector4 left, float right)
{
Vector4 result;
Multiply(ref left, right, out result);
return result;
}
public static Vector4 operator *(float left, Vector4 right)
{
Vector4 result;
Multiply(ref right, left, out result);
return result;
}
public static Vector4 operator /(Vector4 left, Vector4 right)
{
Vector4 result;
Divide(ref left, ref right, out result);
return result;
}
public static Vector4 operator /(Vector4 left, float right)
{
Vector4 result;
Divide(ref left, right, out result);
return result;
}
public static bool operator ==(Vector4 left, Vector4 right)
{
return left.Equals(right);
}
public static bool operator !=(Vector4 left, Vector4 right)
{
return !left.Equals(right);
}
public static Vector4 operator -(Vector4 left)
{
return new Vector4(-left.X, -left.Y, -left.Z, -left.W);
}
public override bool Equals(object obj)
{
if (obj is Vector4)
return this.Equals((Vector4)obj);
else
return false;
}
public bool Equals(Vector4 other)
{
return (X == other.X && Y == other.Y && Z == other.Z && W == other.W);
}
public override int GetHashCode()
{
return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode();
}
public override string ToString()
{
return String.Format("{{X:{0} Y:{1} Z:{2} W:{3}}}", X, Y, Z, W);
}
}
}