Extract From https://github.com/zhu1987/TrueSync/tree/master/Assets/TrueSync/Engine/Math

.gitignore

@@ -0,0 +1,362 @@
+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore
+
+# User-specific files
+*.rsuser
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Mono auto generated files
+mono_crash.*
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+[Ww][Ii][Nn]32/
+[Aa][Rr][Mm]/
+[Aa][Rr][Mm]64/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+[Ll]ogs/
+
+# Visual Studio 2015/2017 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# Visual Studio 2017 auto generated files
+Generated\ Files/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUnit
+*.VisualState.xml
+TestResult.xml
+nunit-*.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+
+# ASP.NET Scaffolding
+ScaffoldingReadMe.txt
+
+# StyleCop
+StyleCopReport.xml
+
+# Files built by Visual Studio
+*_i.c
+*_p.c
+*_h.h
+*.ilk
+*.meta
+*.obj
+*.iobj
+*.pch
+*.pdb
+*.ipdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*_wpftmp.csproj
+*.log
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# Visual Studio Trace Files
+*.e2e
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Coverlet is a free, cross platform Code Coverage Tool
+coverage*.json
+coverage*.xml
+coverage*.info
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# NuGet Symbol Packages
+*.snupkg
+# The packages folder can be ignored because of Package Restore
+**/[Pp]ackages/*
+# except build/, which is used as an MSBuild target.
+!**/[Pp]ackages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/[Pp]ackages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+*.appxbundle
+*.appxupload
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!?*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Including strong name files can present a security risk
+# (https://github.com/github/gitignore/pull/2483#issue-259490424)
+#*.snk
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+ServiceFabricBackup/
+*.rptproj.bak
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+*.rptproj.rsuser
+*- [Bb]ackup.rdl
+*- [Bb]ackup ([0-9]).rdl
+*- [Bb]ackup ([0-9][0-9]).rdl
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# CodeRush personal settings
+.cr/personal
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+# OpenCover UI analysis results
+OpenCover/
+
+# Azure Stream Analytics local run output
+ASALocalRun/
+
+# MSBuild Binary and Structured Log
+*.binlog
+
+# NVidia Nsight GPU debugger configuration file
+*.nvuser
+
+# MFractors (Xamarin productivity tool) working folder
+.mfractor/
+
+# Local History for Visual Studio
+.localhistory/
+
+# BeatPulse healthcheck temp database
+healthchecksdb
+
+# Backup folder for Package Reference Convert tool in Visual Studio 2017
+MigrationBackup/
+
+# Ionide (cross platform F# VS Code tools) working folder
+.ionide/
+
+# Fody - auto-generated XML schema
+FodyWeavers.xsd

TrueSync-Math.sln

@@ -0,0 +1,25 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 15
+VisualStudioVersion = 15.0.28307.1382
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "TrueSync-Math", "TrueSync-Math\TrueSync-Math.csproj", "{7F9D0971-ECEA-4E83-97A1-888839852D10}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{7F9D0971-ECEA-4E83-97A1-888839852D10}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{7F9D0971-ECEA-4E83-97A1-888839852D10}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{7F9D0971-ECEA-4E83-97A1-888839852D10}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{7F9D0971-ECEA-4E83-97A1-888839852D10}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {642F8F9F-8584-4BB7-88C8-E2B8E47A86D7}
+	EndGlobalSection
+EndGlobal

TrueSync-Math/Math/TSMath.cs

@@ -0,0 +1,533 @@
+using System;
+/* Copyright (C) <2009-2011> <Thorben Linneweber, Jitter Physics>
+* 
+*  This software is provided 'as-is', without any express or implied
+*  warranty.  In no event will the authors be held liable for any damages
+*  arising from the use of this software.
+*
+*  Permission is granted to anyone to use this software for any purpose,
+*  including commercial applications, and to alter it and redistribute it
+*  freely, subject to the following restrictions:
+*
+*  1. The origin of this software must not be misrepresented; you must not
+*      claim that you wrote the original software. If you use this software
+*      in a product, an acknowledgment in the product documentation would be
+*      appreciated but is not required.
+*  2. Altered source versions must be plainly marked as such, and must not be
+*      misrepresented as being the original software.
+*  3. This notice may not be removed or altered from any source distribution. 
+*/
+
+namespace TrueSync {
+
+    /// <summary>
+    /// Contains common math operations.
+    /// </summary>
+    public sealed class TSMath {
+
+        /// <summary>
+        /// PI constant.
+        /// </summary>
+        public static FP Pi = FP.Pi;
+
+        /**
+        *  @brief PI over 2 constant.
+        **/
+        public static FP PiOver2 = FP.PiOver2;
+
+        /// <summary>
+        /// A small value often used to decide if numeric 
+        /// results are zero.
+        /// </summary>
+		public static FP Epsilon = FP.Epsilon;
+
+        /**
+        *  @brief Degree to radians constant.
+        **/
+        public static FP Deg2Rad = FP.Deg2Rad;
+
+        /**
+        *  @brief Radians to degree constant.
+        **/
+        public static FP Rad2Deg = FP.Rad2Deg;
+
+
+        /**
+         * @brief FP infinity.
+         * */
+        public static FP Infinity = FP.MaxValue;
+
+        /// <summary>
+        /// Gets the square root.
+        /// </summary>
+        /// <param name="number">The number to get the square root from.</param>
+        /// <returns></returns>
+        #region public static FP Sqrt(FP number)
+        public static FP Sqrt(FP number) {
+            return FP.Sqrt(number);
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the maximum number of two values.
+        /// </summary>
+        /// <param name="val1">The first value.</param>
+        /// <param name="val2">The second value.</param>
+        /// <returns>Returns the largest value.</returns>
+        #region public static FP Max(FP val1, FP val2)
+        public static FP Max(FP val1, FP val2) {
+            return (val1 > val2) ? val1 : val2;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the minimum number of two values.
+        /// </summary>
+        /// <param name="val1">The first value.</param>
+        /// <param name="val2">The second value.</param>
+        /// <returns>Returns the smallest value.</returns>
+        #region public static FP Min(FP val1, FP val2)
+        public static FP Min(FP val1, FP val2) {
+            return (val1 < val2) ? val1 : val2;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the maximum number of three values.
+        /// </summary>
+        /// <param name="val1">The first value.</param>
+        /// <param name="val2">The second value.</param>
+        /// <param name="val3">The third value.</param>
+        /// <returns>Returns the largest value.</returns>
+        #region public static FP Max(FP val1, FP val2,FP val3)
+        public static FP Max(FP val1, FP val2, FP val3) {
+            FP max12 = (val1 > val2) ? val1 : val2;
+            return (max12 > val3) ? max12 : val3;
+        }
+        #endregion
+
+        /// <summary>
+        /// Returns a number which is within [min,max]
+        /// </summary>
+        /// <param name="value">The value to clamp.</param>
+        /// <param name="min">The minimum value.</param>
+        /// <param name="max">The maximum value.</param>
+        /// <returns>The clamped value.</returns>
+        #region public static FP Clamp(FP value, FP min, FP max)
+        public static FP Clamp(FP value, FP min, FP max) {
+            if (value < min)
+            {
+                value = min;
+                return value;
+            }
+            if (value > max)
+            {
+                value = max;
+            }
+            return value;
+        }
+        #endregion
+
+        /// <summary>
+        /// Returns a number which is within [FP.Zero, FP.One]
+        /// </summary>
+        /// <param name="value">The value to clamp.</param>
+        /// <returns>The clamped value.</returns>
+        public static FP Clamp01(FP value)
+        {
+            if (value < FP.Zero)
+                return FP.Zero;
+
+            if (value > FP.One)
+                return FP.One;
+
+            return value;
+        }
+
+        /// <summary>
+        /// Changes every sign of the matrix entry to '+'
+        /// </summary>
+        /// <param name="matrix">The matrix.</param>
+        /// <param name="result">The absolute matrix.</param>
+        #region public static void Absolute(ref JMatrix matrix,out JMatrix result)
+        public static void Absolute(ref TSMatrix matrix, out TSMatrix result) {
+            result.M11 = FP.Abs(matrix.M11);
+            result.M12 = FP.Abs(matrix.M12);
+            result.M13 = FP.Abs(matrix.M13);
+            result.M21 = FP.Abs(matrix.M21);
+            result.M22 = FP.Abs(matrix.M22);
+            result.M23 = FP.Abs(matrix.M23);
+            result.M31 = FP.Abs(matrix.M31);
+            result.M32 = FP.Abs(matrix.M32);
+            result.M33 = FP.Abs(matrix.M33);
+        }
+        #endregion
+
+        /// <summary>
+        /// Returns the sine of value.
+        /// </summary>
+        public static FP Sin(FP value) {
+            return FP.Sin(value);
+        }
+
+        /// <summary>
+        /// Returns the cosine of value.
+        /// </summary>
+        public static FP Cos(FP value) {
+            return FP.Cos(value);
+        }
+
+        /// <summary>
+        /// Returns the tan of value.
+        /// </summary>
+        public static FP Tan(FP value) {
+            return FP.Tan(value);
+        }
+
+        /// <summary>
+        /// Returns the arc sine of value.
+        /// </summary>
+        public static FP Asin(FP value) {
+            return FP.Asin(value);
+        }
+
+        /// <summary>
+        /// Returns the arc cosine of value.
+        /// </summary>
+        public static FP Acos(FP value) {
+            return FP.Acos(value);
+        }
+
+        /// <summary>
+        /// Returns the arc tan of value.
+        /// </summary>
+        public static FP Atan(FP value) {
+            return FP.Atan(value);
+        }
+
+        /// <summary>
+        /// Returns the arc tan of coordinates x-y.
+        /// </summary>
+        public static FP Atan2(FP y, FP x) {
+            return FP.Atan2(y, x);
+        }
+
+        /// <summary>
+        /// Returns the largest integer less than or equal to the specified number.
+        /// </summary>
+        public static FP Floor(FP value) {
+            return FP.Floor(value);
+        }
+
+        /// <summary>
+        /// Returns the smallest integral value that is greater than or equal to the specified number.
+        /// </summary>
+        public static FP Ceiling(FP value) {
+            return value;
+        }
+
+        /// <summary>
+        /// Rounds a value to the nearest integral value.
+        /// If the value is halfway between an even and an uneven value, returns the even value.
+        /// </summary>
+        public static FP Round(FP value) {
+            return FP.Round(value);
+        }
+
+        /// <summary>
+        /// Returns a number indicating the sign of a Fix64 number.
+        /// Returns 1 if the value is positive, 0 if is 0, and -1 if it is negative.
+        /// </summary>
+        public static int Sign(FP value) {
+            return FP.Sign(value);
+        }
+
+        /// <summary>
+        /// Returns the absolute value of a Fix64 number.
+        /// Note: Abs(Fix64.MinValue) == Fix64.MaxValue.
+        /// </summary>
+        public static FP Abs(FP value) {
+            return FP.Abs(value);                
+        }
+
+        public static FP Barycentric(FP value1, FP value2, FP value3, FP amount1, FP amount2) {
+            return value1 + (value2 - value1) * amount1 + (value3 - value1) * amount2;
+        }
+
+        public static FP CatmullRom(FP value1, FP value2, FP value3, FP value4, FP amount) {
+            // Using formula from http://www.mvps.org/directx/articles/catmull/
+            // Internally using FPs not to lose precission
+            FP amountSquared = amount * amount;
+            FP amountCubed = amountSquared * amount;
+            return (FP)(0.5 * (2.0 * value2 +
+                                 (value3 - value1) * amount +
+                                 (2.0 * value1 - 5.0 * value2 + 4.0 * value3 - value4) * amountSquared +
+                                 (3.0 * value2 - value1 - 3.0 * value3 + value4) * amountCubed));
+        }
+
+        public static FP Distance(FP value1, FP value2) {
+            return FP.Abs(value1 - value2);
+        }
+
+        public static FP Hermite(FP value1, FP tangent1, FP value2, FP tangent2, FP amount) {
+            // All transformed to FP not to lose precission
+            // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity
+            FP v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result;
+            FP sCubed = s * s * s;
+            FP sSquared = s * s;
+
+            if (amount == 0f)
+                result = value1;
+            else if (amount == 1f)
+                result = value2;
+            else
+                result = (2 * v1 - 2 * v2 + t2 + t1) * sCubed +
+                         (3 * v2 - 3 * v1 - 2 * t1 - t2) * sSquared +
+                         t1 * s +
+                         v1;
+            return (FP)result;
+        }
+
+        public static FP Lerp(FP value1, FP value2, FP amount) {
+            return value1 + (value2 - value1) * Clamp01(amount);
+        }
+
+        public static FP InverseLerp(FP value1, FP value2, FP amount) {
+            if (value1 != value2)
+                return Clamp01((amount - value1) / (value2 - value1));
+            return FP.Zero;
+        }
+
+        public static FP SmoothStep(FP value1, FP value2, FP amount) {
+            // It is expected that 0 < amount < 1
+            // If amount < 0, return value1
+            // If amount > 1, return value2
+            FP result = Clamp(amount, 0f, 1f);
+            result = Hermite(value1, 0f, value2, 0f, result);
+            return result;
+        }
+
+
+        /// <summary>
+        /// Returns 2 raised to the specified power.
+        /// Provides at least 6 decimals of accuracy.
+        /// </summary>
+        internal static FP Pow2(FP x)
+        {
+            if (x.RawValue == 0)
+            {
+                return FP.One;
+            }
+
+            // Avoid negative arguments by exploiting that exp(-x) = 1/exp(x).
+            bool neg = x.RawValue < 0;
+            if (neg)
+            {
+                x = -x;
+            }
+
+            if (x == FP.One)
+            {
+                return neg ? FP.One / (FP)2 : (FP)2;
+            }
+            if (x >= FP.Log2Max)
+            {
+                return neg ? FP.One / FP.MaxValue : FP.MaxValue;
+            }
+            if (x <= FP.Log2Min)
+            {
+                return neg ? FP.MaxValue : FP.Zero;
+            }
+
+            /* The algorithm is based on the power series for exp(x):
+             * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition
+             * 
+             * From term n, we get term n+1 by multiplying with x/n.
+             * When the sum term drops to zero, we can stop summing.
+             */
+
+            int integerPart = (int)Floor(x);
+            // Take fractional part of exponent
+            x = FP.FromRaw(x.RawValue & 0x00000000FFFFFFFF);
+
+            var result = FP.One;
+            var term = FP.One;
+            int i = 1;
+            while (term.RawValue != 0)
+            {
+                term = FP.FastMul(FP.FastMul(x, term), FP.Ln2) / (FP)i;
+                result += term;
+                i++;
+            }
+
+            result = FP.FromRaw(result.RawValue << integerPart);
+            if (neg)
+            {
+                result = FP.One / result;
+            }
+
+            return result;
+        }
+
+        /// <summary>
+        /// Returns the base-2 logarithm of a specified number.
+        /// Provides at least 9 decimals of accuracy.
+        /// </summary>
+        /// <exception cref="ArgumentOutOfRangeException">
+        /// The argument was non-positive
+        /// </exception>
+        internal static FP Log2(FP x)
+        {
+            if (x.RawValue <= 0)
+            {
+                throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x");
+            }
+
+            // This implementation is based on Clay. S. Turner's fast binary logarithm
+            // algorithm (C. S. Turner,  "A Fast Binary Logarithm Algorithm", IEEE Signal
+            //     Processing Mag., pp. 124,140, Sep. 2010.)
+
+            long b = 1U << (FP.FRACTIONAL_PLACES - 1);
+            long y = 0;
+
+            long rawX = x.RawValue;
+            while (rawX < FP.ONE)
+            {
+                rawX <<= 1;
+                y -= FP.ONE;
+            }
+
+            while (rawX >= (FP.ONE << 1))
+            {
+                rawX >>= 1;
+                y += FP.ONE;
+            }
+
+            var z = FP.FromRaw(rawX);
+
+            for (int i = 0; i < FP.FRACTIONAL_PLACES; i++)
+            {
+                z = FP.FastMul(z, z);
+                if (z.RawValue >= (FP.ONE << 1))
+                {
+                    z = FP.FromRaw(z.RawValue >> 1);
+                    y += b;
+                }
+                b >>= 1;
+            }
+
+            return FP.FromRaw(y);
+        }
+
+        /// <summary>
+        /// Returns the natural logarithm of a specified number.
+        /// Provides at least 7 decimals of accuracy.
+        /// </summary>
+        /// <exception cref="ArgumentOutOfRangeException">
+        /// The argument was non-positive
+        /// </exception>
+        public static FP Ln(FP x)
+        {
+            return FP.FastMul(Log2(x), FP.Ln2);
+        }
+
+        /// <summary>
+        /// Returns a specified number raised to the specified power.
+        /// Provides about 5 digits of accuracy for the result.
+        /// </summary>
+        /// <exception cref="DivideByZeroException">
+        /// The base was zero, with a negative exponent
+        /// </exception>
+        /// <exception cref="ArgumentOutOfRangeException">
+        /// The base was negative, with a non-zero exponent
+        /// </exception>
+        public static FP Pow(FP b, FP exp)
+        {
+            if (b == FP.One)
+            {
+                return FP.One;
+            }
+
+            if (exp.RawValue == 0)
+            {
+                return FP.One;
+            }
+
+            if (b.RawValue == 0)
+            {
+                if (exp.RawValue < 0)
+                {
+                    //throw new DivideByZeroException();
+                    return FP.MaxValue;
+                }
+                return FP.Zero;
+            }
+
+            FP log2 = Log2(b);
+            return Pow2(exp * log2);
+        }
+
+        public static FP MoveTowards(FP current, FP target, FP maxDelta)
+        {
+            if (Abs(target - current) <= maxDelta)
+                return target;
+            return (current + (Sign(target - current)) * maxDelta);
+        }
+
+        public static FP Repeat(FP t, FP length)
+        {
+            return (t - (Floor(t / length) * length));
+        }
+
+        public static FP DeltaAngle(FP current, FP target)
+        {
+            FP num = Repeat(target - current, (FP)360f);
+            if (num > (FP)180f)
+            {
+                num -= (FP)360f;
+            }
+            return num;
+        }
+
+        public static FP MoveTowardsAngle(FP current, FP target, float maxDelta)
+        {
+            target = current + DeltaAngle(current, target);
+            return MoveTowards(current, target, maxDelta);
+        }
+
+        public static FP SmoothDamp(FP current, FP target, ref FP currentVelocity, FP smoothTime, FP maxSpeed)
+        {
+            FP deltaTime = FP.EN2;
+            return SmoothDamp(current, target, ref currentVelocity, smoothTime, maxSpeed, deltaTime);
+        }
+
+        public static FP SmoothDamp(FP current, FP target, ref FP currentVelocity, FP smoothTime)
+        {
+            FP deltaTime = FP.EN2;
+            FP positiveInfinity = -FP.MaxValue;
+            return SmoothDamp(current, target, ref currentVelocity, smoothTime, positiveInfinity, deltaTime);
+        }
+
+        public static FP SmoothDamp(FP current, FP target, ref FP currentVelocity, FP smoothTime, FP maxSpeed, FP deltaTime)
+        {
+            smoothTime = Max(FP.EN4, smoothTime);
+            FP num = (FP)2f / smoothTime;
+            FP num2 = num * deltaTime;
+            FP num3 = FP.One / (((FP.One + num2) + (((FP)0.48f * num2) * num2)) + ((((FP)0.235f * num2) * num2) * num2));
+            FP num4 = current - target;
+            FP num5 = target;
+            FP max = maxSpeed * smoothTime;
+            num4 = Clamp(num4, -max, max);
+            target = current - num4;
+            FP num7 = (currentVelocity + (num * num4)) * deltaTime;
+            currentVelocity = (currentVelocity - (num * num7)) * num3;
+            FP num8 = target + ((num4 + num7) * num3);
+            if (((num5 - current) > FP.Zero) == (num8 > num5))
+            {
+                num8 = num5;
+                currentVelocity = (num8 - num5) / deltaTime;
+            }
+            return num8;
+        }
+    }
+}

TrueSync-Math/Math/TSMatrix.cs

@@ -0,0 +1,706 @@
+/* Copyright (C) <2009-2011> <Thorben Linneweber, Jitter Physics>
+* 
+*  This software is provided 'as-is', without any express or implied
+*  warranty.  In no event will the authors be held liable for any damages
+*  arising from the use of this software.
+*
+*  Permission is granted to anyone to use this software for any purpose,
+*  including commercial applications, and to alter it and redistribute it
+*  freely, subject to the following restrictions:
+*
+*  1. The origin of this software must not be misrepresented; you must not
+*      claim that you wrote the original software. If you use this software
+*      in a product, an acknowledgment in the product documentation would be
+*      appreciated but is not required.
+*  2. Altered source versions must be plainly marked as such, and must not be
+*      misrepresented as being the original software.
+*  3. This notice may not be removed or altered from any source distribution. 
+*/
+
+namespace TrueSync
+{
+
+    /// <summary>
+    /// 3x3 Matrix.
+    /// </summary>
+    public struct TSMatrix
+    {
+        /// <summary>
+        /// M11
+        /// </summary>
+        public FP M11; // 1st row vector
+        /// <summary>
+        /// M12
+        /// </summary>
+        public FP M12;
+        /// <summary>
+        /// M13
+        /// </summary>
+        public FP M13;
+        /// <summary>
+        /// M21
+        /// </summary>
+        public FP M21; // 2nd row vector
+        /// <summary>
+        /// M22
+        /// </summary>
+        public FP M22;
+        /// <summary>
+        /// M23
+        /// </summary>
+        public FP M23;
+        /// <summary>
+        /// M31
+        /// </summary>
+        public FP M31; // 3rd row vector
+        /// <summary>
+        /// M32
+        /// </summary>
+        public FP M32;
+        /// <summary>
+        /// M33
+        /// </summary>
+        public FP M33;
+
+        internal static TSMatrix InternalIdentity;
+
+        /// <summary>
+        /// Identity matrix.
+        /// </summary>
+        public static readonly TSMatrix Identity;
+        public static readonly TSMatrix Zero;
+
+        static TSMatrix()
+        {
+            Zero = new TSMatrix();
+
+            Identity = new TSMatrix();
+            Identity.M11 = FP.One;
+            Identity.M22 = FP.One;
+            Identity.M33 = FP.One;
+
+            InternalIdentity = Identity;
+        }
+
+        public TSVector eulerAngles {
+            get {
+                TSVector result = new TSVector();
+
+                result.x = TSMath.Atan2(M32, M33) * FP.Rad2Deg;
+                result.y = TSMath.Atan2(-M31, TSMath.Sqrt(M32 * M32 + M33 * M33)) * FP.Rad2Deg;
+                result.z = TSMath.Atan2(M21, M11) * FP.Rad2Deg;
+
+                return result * -1;
+            }
+        }
+
+        public static TSMatrix CreateFromYawPitchRoll(FP yaw, FP pitch, FP roll)
+        {
+            TSMatrix matrix;
+            TSQuaternion quaternion;
+            TSQuaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion);
+            CreateFromQuaternion(ref quaternion, out matrix);
+            return matrix;
+        }
+
+        public static TSMatrix CreateRotationX(FP radians)
+        {
+            TSMatrix matrix;
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            matrix.M11 = FP.One;
+            matrix.M12 = FP.Zero;
+            matrix.M13 = FP.Zero;
+            matrix.M21 = FP.Zero;
+            matrix.M22 = num2;
+            matrix.M23 = num;
+            matrix.M31 = FP.Zero;
+            matrix.M32 = -num;
+            matrix.M33 = num2;
+            return matrix;
+        }
+
+        public static void CreateRotationX(FP radians, out TSMatrix result)
+        {
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            result.M11 = FP.One;
+            result.M12 = FP.Zero;
+            result.M13 = FP.Zero;
+            result.M21 = FP.Zero;
+            result.M22 = num2;
+            result.M23 = num;
+            result.M31 = FP.Zero;
+            result.M32 = -num;
+            result.M33 = num2;
+        }
+
+        public static TSMatrix CreateRotationY(FP radians)
+        {
+            TSMatrix matrix;
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            matrix.M11 = num2;
+            matrix.M12 = FP.Zero;
+            matrix.M13 = -num;
+            matrix.M21 = FP.Zero;
+            matrix.M22 = FP.One;
+            matrix.M23 = FP.Zero;
+            matrix.M31 = num;
+            matrix.M32 = FP.Zero;
+            matrix.M33 = num2;
+            return matrix;
+        }
+
+        public static void CreateRotationY(FP radians, out TSMatrix result)
+        {
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            result.M11 = num2;
+            result.M12 = FP.Zero;
+            result.M13 = -num;
+            result.M21 = FP.Zero;
+            result.M22 = FP.One;
+            result.M23 = FP.Zero;
+            result.M31 = num;
+            result.M32 = FP.Zero;
+            result.M33 = num2;
+        }
+
+        public static TSMatrix CreateRotationZ(FP radians)
+        {
+            TSMatrix matrix;
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            matrix.M11 = num2;
+            matrix.M12 = num;
+            matrix.M13 = FP.Zero;
+            matrix.M21 = -num;
+            matrix.M22 = num2;
+            matrix.M23 = FP.Zero;
+            matrix.M31 = FP.Zero;
+            matrix.M32 = FP.Zero;
+            matrix.M33 = FP.One;
+            return matrix;
+        }
+
+
+        public static void CreateRotationZ(FP radians, out TSMatrix result)
+        {
+            FP num2 = FP.Cos(radians);
+            FP num = FP.Sin(radians);
+            result.M11 = num2;
+            result.M12 = num;
+            result.M13 = FP.Zero;
+            result.M21 = -num;
+            result.M22 = num2;
+            result.M23 = FP.Zero;
+            result.M31 = FP.Zero;
+            result.M32 = FP.Zero;
+            result.M33 = FP.One;
+        }
+
+        /// <summary>
+        /// Initializes a new instance of the matrix structure.
+        /// </summary>
+        /// <param name="m11">m11</param>
+        /// <param name="m12">m12</param>
+        /// <param name="m13">m13</param>
+        /// <param name="m21">m21</param>
+        /// <param name="m22">m22</param>
+        /// <param name="m23">m23</param>
+        /// <param name="m31">m31</param>
+        /// <param name="m32">m32</param>
+        /// <param name="m33">m33</param>
+        #region public JMatrix(FP m11, FP m12, FP m13, FP m21, FP m22, FP m23,FP m31, FP m32, FP m33)
+        public TSMatrix(FP m11, FP m12, FP m13, FP m21, FP m22, FP m23,FP m31, FP m32, FP m33)
+        {
+            this.M11 = m11;
+            this.M12 = m12;
+            this.M13 = m13;
+            this.M21 = m21;
+            this.M22 = m22;
+            this.M23 = m23;
+            this.M31 = m31;
+            this.M32 = m32;
+            this.M33 = m33;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the determinant of the matrix.
+        /// </summary>
+        /// <returns>The determinant of the matrix.</returns>
+        #region public FP Determinant()
+        //public FP Determinant()
+        //{
+        //    return M11 * M22 * M33 -M11 * M23 * M32 -M12 * M21 * M33 +M12 * M23 * M31 + M13 * M21 * M32 - M13 * M22 * M31;
+        //}
+        #endregion
+
+        /// <summary>
+        /// Multiply two matrices. Notice: matrix multiplication is not commutative.
+        /// </summary>
+        /// <param name="matrix1">The first matrix.</param>
+        /// <param name="matrix2">The second matrix.</param>
+        /// <returns>The product of both matrices.</returns>
+        #region public static JMatrix Multiply(JMatrix matrix1, JMatrix matrix2)
+        public static TSMatrix Multiply(TSMatrix matrix1, TSMatrix matrix2)
+        {
+            TSMatrix result;
+            TSMatrix.Multiply(ref matrix1, ref matrix2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Multiply two matrices. Notice: matrix multiplication is not commutative.
+        /// </summary>
+        /// <param name="matrix1">The first matrix.</param>
+        /// <param name="matrix2">The second matrix.</param>
+        /// <param name="result">The product of both matrices.</param>
+        public static void Multiply(ref TSMatrix matrix1, ref TSMatrix matrix2, out TSMatrix result)
+        {
+            FP num0 = ((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31);
+            FP num1 = ((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32);
+            FP num2 = ((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33);
+            FP num3 = ((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31);
+            FP num4 = ((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32);
+            FP num5 = ((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33);
+            FP num6 = ((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31);
+            FP num7 = ((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32);
+            FP num8 = ((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33);
+
+            result.M11 = num0;
+            result.M12 = num1;
+            result.M13 = num2;
+            result.M21 = num3;
+            result.M22 = num4;
+            result.M23 = num5;
+            result.M31 = num6;
+            result.M32 = num7;
+            result.M33 = num8;
+        }
+        #endregion
+
+        /// <summary>
+        /// Matrices are added.
+        /// </summary>
+        /// <param name="matrix1">The first matrix.</param>
+        /// <param name="matrix2">The second matrix.</param>
+        /// <returns>The sum of both matrices.</returns>
+        #region public static JMatrix Add(JMatrix matrix1, JMatrix matrix2)
+        public static TSMatrix Add(TSMatrix matrix1, TSMatrix matrix2)
+        {
+            TSMatrix result;
+            TSMatrix.Add(ref matrix1, ref matrix2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Matrices are added.
+        /// </summary>
+        /// <param name="matrix1">The first matrix.</param>
+        /// <param name="matrix2">The second matrix.</param>
+        /// <param name="result">The sum of both matrices.</param>
+        public static void Add(ref TSMatrix matrix1, ref TSMatrix matrix2, out TSMatrix result)
+        {
+            result.M11 = matrix1.M11 + matrix2.M11;
+            result.M12 = matrix1.M12 + matrix2.M12;
+            result.M13 = matrix1.M13 + matrix2.M13;
+            result.M21 = matrix1.M21 + matrix2.M21;
+            result.M22 = matrix1.M22 + matrix2.M22;
+            result.M23 = matrix1.M23 + matrix2.M23;
+            result.M31 = matrix1.M31 + matrix2.M31;
+            result.M32 = matrix1.M32 + matrix2.M32;
+            result.M33 = matrix1.M33 + matrix2.M33;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the inverse of a give matrix.
+        /// </summary>
+        /// <param name="matrix">The matrix to invert.</param>
+        /// <returns>The inverted JMatrix.</returns>
+        #region public static JMatrix Inverse(JMatrix matrix)
+        public static TSMatrix Inverse(TSMatrix matrix)
+        {
+            TSMatrix result;
+            TSMatrix.Inverse(ref matrix, out result);
+            return result;
+        }
+
+        public FP Determinant()
+        {
+            return M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 -
+                   M31 * M22 * M13 - M32 * M23 * M11 - M33 * M21 * M12;
+        }
+
+        public static void Invert(ref TSMatrix matrix, out TSMatrix result)
+        {
+            FP determinantInverse = 1 / matrix.Determinant();
+            FP m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse;
+            FP m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse;
+            FP m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse;
+
+            FP m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse;
+            FP m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse;
+            FP m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse;
+
+            FP m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse;
+            FP m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse;
+            FP m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse;
+
+            result.M11 = m11;
+            result.M12 = m12;
+            result.M13 = m13;
+
+            result.M21 = m21;
+            result.M22 = m22;
+            result.M23 = m23;
+
+            result.M31 = m31;
+            result.M32 = m32;
+            result.M33 = m33;
+        }
+
+        /// <summary>
+        /// Calculates the inverse of a give matrix.
+        /// </summary>
+        /// <param name="matrix">The matrix to invert.</param>
+        /// <param name="result">The inverted JMatrix.</param>
+        public static void Inverse(ref TSMatrix matrix, out TSMatrix result)
+        {
+			FP det = 1024 * matrix.M11 * matrix.M22 * matrix.M33 -
+				1024 * matrix.M11 * matrix.M23 * matrix.M32 -
+				1024 * matrix.M12 * matrix.M21 * matrix.M33 +
+				1024 * matrix.M12 * matrix.M23 * matrix.M31 +
+				1024 * matrix.M13 * matrix.M21 * matrix.M32 -
+				1024 * matrix.M13 * matrix.M22 * matrix.M31;
+
+			FP num11 =1024* matrix.M22 * matrix.M33 - 1024*matrix.M23 * matrix.M32;
+			FP num12 =1024* matrix.M13 * matrix.M32 -1024* matrix.M12 * matrix.M33;
+			FP num13 =1024* matrix.M12 * matrix.M23 -1024* matrix.M22 * matrix.M13;
+
+			FP num21 =1024* matrix.M23 * matrix.M31 -1024* matrix.M33 * matrix.M21;
+			FP num22 =1024* matrix.M11 * matrix.M33 -1024* matrix.M31 * matrix.M13;
+			FP num23 =1024* matrix.M13 * matrix.M21 -1024* matrix.M23 * matrix.M11;
+
+			FP num31 =1024* matrix.M21 * matrix.M32 - 1024* matrix.M31 * matrix.M22;
+			FP num32 =1024* matrix.M12 * matrix.M31 - 1024* matrix.M32 * matrix.M11;
+			FP num33 =1024* matrix.M11 * matrix.M22 - 1024*matrix.M21 * matrix.M12;
+
+			if(det == 0){
+				result.M11 = FP.PositiveInfinity;
+				result.M12 = FP.PositiveInfinity;
+				result.M13 = FP.PositiveInfinity;
+				result.M21 = FP.PositiveInfinity;
+				result.M22 = FP.PositiveInfinity;
+				result.M23 = FP.PositiveInfinity;
+				result.M31 = FP.PositiveInfinity;
+				result.M32 = FP.PositiveInfinity;
+				result.M33 = FP.PositiveInfinity;
+			} else{
+				result.M11 = num11 / det;
+				result.M12 = num12 / det;
+				result.M13 = num13 / det;
+				result.M21 = num21 / det;
+				result.M22 = num22 / det;
+				result.M23 = num23 / det;
+				result.M31 = num31 / det;
+				result.M32 = num32 / det;
+				result.M33 = num33 / det;
+			}
+            
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiply a matrix by a scalefactor.
+        /// </summary>
+        /// <param name="matrix1">The matrix.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>A JMatrix multiplied by the scale factor.</returns>
+        #region public static JMatrix Multiply(JMatrix matrix1, FP scaleFactor)
+        public static TSMatrix Multiply(TSMatrix matrix1, FP scaleFactor)
+        {
+            TSMatrix result;
+            TSMatrix.Multiply(ref matrix1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Multiply a matrix by a scalefactor.
+        /// </summary>
+        /// <param name="matrix1">The matrix.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <param name="result">A JMatrix multiplied by the scale factor.</param>
+        public static void Multiply(ref TSMatrix matrix1, FP scaleFactor, out TSMatrix result)
+        {
+            FP num = scaleFactor;
+            result.M11 = matrix1.M11 * num;
+            result.M12 = matrix1.M12 * num;
+            result.M13 = matrix1.M13 * num;
+            result.M21 = matrix1.M21 * num;
+            result.M22 = matrix1.M22 * num;
+            result.M23 = matrix1.M23 * num;
+            result.M31 = matrix1.M31 * num;
+            result.M32 = matrix1.M32 * num;
+            result.M33 = matrix1.M33 * num;
+        }
+        #endregion
+
+        /// <summary>
+        /// Creates a JMatrix representing an orientation from a quaternion.
+        /// </summary>
+        /// <param name="quaternion">The quaternion the matrix should be created from.</param>
+        /// <returns>JMatrix representing an orientation.</returns>
+        #region public static JMatrix CreateFromQuaternion(JQuaternion quaternion)
+
+		public static TSMatrix CreateFromLookAt(TSVector position, TSVector target){
+			TSMatrix result;
+			LookAt (target - position, TSVector.up, out result);
+			return result;
+		}
+
+        public static TSMatrix LookAt(TSVector forward, TSVector upwards) {
+            TSMatrix result;
+            LookAt(forward, upwards, out result);
+
+            return result;
+        }
+
+        public static void LookAt(TSVector forward, TSVector upwards, out TSMatrix result) {
+            TSVector zaxis = forward; zaxis.Normalize();
+            TSVector xaxis = TSVector.Cross(upwards, zaxis); xaxis.Normalize();
+            TSVector yaxis = TSVector.Cross(zaxis, xaxis);
+
+            result.M11 = xaxis.x;
+            result.M21 = yaxis.x;
+            result.M31 = zaxis.x;
+            result.M12 = xaxis.y;
+            result.M22 = yaxis.y;
+            result.M32 = zaxis.y;
+            result.M13 = xaxis.z;
+            result.M23 = yaxis.z;
+            result.M33 = zaxis.z;
+        }
+
+        public static TSMatrix CreateFromQuaternion(TSQuaternion quaternion)
+        {
+            TSMatrix result;
+            TSMatrix.CreateFromQuaternion(ref quaternion,out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Creates a JMatrix representing an orientation from a quaternion.
+        /// </summary>
+        /// <param name="quaternion">The quaternion the matrix should be created from.</param>
+        /// <param name="result">JMatrix representing an orientation.</param>
+        public static void CreateFromQuaternion(ref TSQuaternion quaternion, out TSMatrix result)
+        {
+            FP num9 = quaternion.x * quaternion.x;
+            FP num8 = quaternion.y * quaternion.y;
+            FP num7 = quaternion.z * quaternion.z;
+            FP num6 = quaternion.x * quaternion.y;
+            FP num5 = quaternion.z * quaternion.w;
+            FP num4 = quaternion.z * quaternion.x;
+            FP num3 = quaternion.y * quaternion.w;
+            FP num2 = quaternion.y * quaternion.z;
+            FP num = quaternion.x * quaternion.w;
+            result.M11 = FP.One - (2 * (num8 + num7));
+            result.M12 = 2 * (num6 + num5);
+            result.M13 = 2 * (num4 - num3);
+            result.M21 = 2 * (num6 - num5);
+            result.M22 = FP.One - (2 * (num7 + num9));
+            result.M23 = 2 * (num2 + num);
+            result.M31 = 2 * (num4 + num3);
+            result.M32 = 2 * (num2 - num);
+            result.M33 = FP.One - (2 * (num8 + num9));
+        }
+        #endregion
+
+        /// <summary>
+        /// Creates the transposed matrix.
+        /// </summary>
+        /// <param name="matrix">The matrix which should be transposed.</param>
+        /// <returns>The transposed JMatrix.</returns>
+        #region public static JMatrix Transpose(JMatrix matrix)
+        public static TSMatrix Transpose(TSMatrix matrix)
+        {
+            TSMatrix result;
+            TSMatrix.Transpose(ref matrix, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Creates the transposed matrix.
+        /// </summary>
+        /// <param name="matrix">The matrix which should be transposed.</param>
+        /// <param name="result">The transposed JMatrix.</param>
+        public static void Transpose(ref TSMatrix matrix, out TSMatrix result)
+        {
+            result.M11 = matrix.M11;
+            result.M12 = matrix.M21;
+            result.M13 = matrix.M31;
+            result.M21 = matrix.M12;
+            result.M22 = matrix.M22;
+            result.M23 = matrix.M32;
+            result.M31 = matrix.M13;
+            result.M32 = matrix.M23;
+            result.M33 = matrix.M33;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies two matrices.
+        /// </summary>
+        /// <param name="value1">The first matrix.</param>
+        /// <param name="value2">The second matrix.</param>
+        /// <returns>The product of both values.</returns>
+        #region public static JMatrix operator *(JMatrix value1,JMatrix value2)
+        public static TSMatrix operator *(TSMatrix value1,TSMatrix value2)
+        {
+            TSMatrix result; TSMatrix.Multiply(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+
+        public FP Trace()
+        {
+            return this.M11 + this.M22 + this.M33;
+        }
+
+        /// <summary>
+        /// Adds two matrices.
+        /// </summary>
+        /// <param name="value1">The first matrix.</param>
+        /// <param name="value2">The second matrix.</param>
+        /// <returns>The sum of both values.</returns>
+        #region public static JMatrix operator +(JMatrix value1, JMatrix value2)
+        public static TSMatrix operator +(TSMatrix value1, TSMatrix value2)
+        {
+            TSMatrix result; TSMatrix.Add(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Subtracts two matrices.
+        /// </summary>
+        /// <param name="value1">The first matrix.</param>
+        /// <param name="value2">The second matrix.</param>
+        /// <returns>The difference of both values.</returns>
+        #region public static JMatrix operator -(JMatrix value1, JMatrix value2)
+        public static TSMatrix operator -(TSMatrix value1, TSMatrix value2)
+        {
+            TSMatrix result; TSMatrix.Multiply(ref value2, -FP.One, out value2);
+            TSMatrix.Add(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        public static bool operator == (TSMatrix value1, TSMatrix value2) {
+            return value1.M11 == value2.M11 &&
+                value1.M12 == value2.M12 &&
+                value1.M13 == value2.M13 &&
+                value1.M21 == value2.M21 &&
+                value1.M22 == value2.M22 &&
+                value1.M23 == value2.M23 &&
+                value1.M31 == value2.M31 &&
+                value1.M32 == value2.M32 &&
+                value1.M33 == value2.M33;
+        }
+
+        public static bool operator != (TSMatrix value1, TSMatrix value2) {
+            return value1.M11 != value2.M11 ||
+                value1.M12 != value2.M12 ||
+                value1.M13 != value2.M13 ||
+                value1.M21 != value2.M21 ||
+                value1.M22 != value2.M22 ||
+                value1.M23 != value2.M23 ||
+                value1.M31 != value2.M31 ||
+                value1.M32 != value2.M32 ||
+                value1.M33 != value2.M33;
+        }
+
+        public override bool Equals(object obj) {
+            if (!(obj is TSMatrix)) return false;
+            TSMatrix other = (TSMatrix) obj;
+
+            return this.M11 == other.M11 &&
+                this.M12 == other.M12 &&
+                this.M13 == other.M13 &&
+                this.M21 == other.M21 &&
+                this.M22 == other.M22 &&
+                this.M23 == other.M23 &&
+                this.M31 == other.M31 &&
+                this.M32 == other.M32 &&
+                this.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();
+        }
+
+        /// <summary>
+        /// Creates a matrix which rotates around the given axis by the given angle.
+        /// </summary>
+        /// <param name="axis">The axis.</param>
+        /// <param name="angle">The angle.</param>
+        /// <param name="result">The resulting rotation matrix</param>
+        #region public static void CreateFromAxisAngle(ref JVector axis, FP angle, out JMatrix result)
+        public static void CreateFromAxisAngle(ref TSVector axis, FP angle, out TSMatrix result)
+        {
+            FP x = axis.x;
+            FP y = axis.y;
+            FP z = axis.z;
+            FP num2 = FP.Sin(angle);
+            FP num = FP.Cos(angle);
+            FP num11 = x * x;
+            FP num10 = y * y;
+            FP num9 = z * z;
+            FP num8 = x * y;
+            FP num7 = x * z;
+            FP num6 = y * z;
+            result.M11 = num11 + (num * (FP.One - num11));
+            result.M12 = (num8 - (num * num8)) + (num2 * z);
+            result.M13 = (num7 - (num * num7)) - (num2 * y);
+            result.M21 = (num8 - (num * num8)) - (num2 * z);
+            result.M22 = num10 + (num * (FP.One - num10));
+            result.M23 = (num6 - (num * num6)) + (num2 * x);
+            result.M31 = (num7 - (num * num7)) + (num2 * y);
+            result.M32 = (num6 - (num * num6)) - (num2 * x);
+            result.M33 = num9 + (num * (FP.One - num9));
+        }
+
+        /// <summary>
+        /// Creates a matrix which rotates around the given axis by the given angle.
+        /// </summary>
+        /// <param name="axis">The axis.</param>
+        /// <param name="angle">The angle.</param>
+        /// <returns>The resulting rotation matrix</returns>
+        public static TSMatrix AngleAxis(FP angle, TSVector axis)
+        {
+            TSMatrix result; CreateFromAxisAngle(ref axis, angle, out result);
+            return result;
+        }
+
+        #endregion
+
+        public override string ToString() {
+            return string.Format("{0}|{1}|{2}|{3}|{4}|{5}|{6}|{7}|{8}", M11.RawValue, M12.RawValue, M13.RawValue, M21.RawValue, M22.RawValue, M23.RawValue, M31.RawValue, M32.RawValue, M33.RawValue);
+        }
+
+    }
+
+}

TrueSync-Math/Math/TSQuaternion.cs

@@ -0,0 +1,522 @@
+/* Copyright (C) <2009-2011> <Thorben Linneweber, Jitter Physics>
+* 
+*  This software is provided 'as-is', without any express or implied
+*  warranty.  In no event will the authors be held liable for any damages
+*  arising from the use of this software.
+*
+*  Permission is granted to anyone to use this software for any purpose,
+*  including commercial applications, and to alter it and redistribute it
+*  freely, subject to the following restrictions:
+*
+*  1. The origin of this software must not be misrepresented; you must not
+*      claim that you wrote the original software. If you use this software
+*      in a product, an acknowledgment in the product documentation would be
+*      appreciated but is not required.
+*  2. Altered source versions must be plainly marked as such, and must not be
+*      misrepresented as being the original software.
+*  3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+
+namespace TrueSync
+{
+
+    /// <summary>
+    /// A Quaternion representing an orientation.
+    /// </summary>
+    [Serializable]
+    public struct TSQuaternion
+    {
+
+        /// <summary>The X component of the quaternion.</summary>
+        public FP x;
+        /// <summary>The Y component of the quaternion.</summary>
+        public FP y;
+        /// <summary>The Z component of the quaternion.</summary>
+        public FP z;
+        /// <summary>The W component of the quaternion.</summary>
+        public FP w;
+
+        public static readonly TSQuaternion identity;
+
+        static TSQuaternion() {
+            identity = new TSQuaternion(0, 0, 0, 1);
+        }
+
+        /// <summary>
+        /// Initializes a new instance of the JQuaternion structure.
+        /// </summary>
+        /// <param name="x">The X component of the quaternion.</param>
+        /// <param name="y">The Y component of the quaternion.</param>
+        /// <param name="z">The Z component of the quaternion.</param>
+        /// <param name="w">The W component of the quaternion.</param>
+        public TSQuaternion(FP x, FP y, FP z, FP w)
+        {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.w = w;
+        }
+
+        public void Set(FP new_x, FP new_y, FP new_z, FP new_w) {
+            this.x = new_x;
+            this.y = new_y;
+            this.z = new_z;
+            this.w = new_w;
+        }
+
+        public void SetFromToRotation(TSVector fromDirection, TSVector toDirection) {
+            TSQuaternion targetRotation = TSQuaternion.FromToRotation(fromDirection, toDirection);
+            this.Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
+        }
+
+        public TSVector eulerAngles {
+            get {
+                TSVector result = new TSVector();
+
+                FP ysqr = y * y;
+                FP t0 = -2.0f * (ysqr + z * z) + 1.0f;
+                FP t1 = +2.0f * (x * y - w * z);
+                FP t2 = -2.0f * (x * z + w * y);
+                FP t3 = +2.0f * (y * z - w * x);
+                FP t4 = -2.0f * (x * x + ysqr) + 1.0f;
+
+                t2 = t2 > 1.0f ? 1.0f : t2;
+                t2 = t2 < -1.0f ? -1.0f : t2;
+
+                result.x = FP.Atan2(t3, t4) * FP.Rad2Deg;
+                result.y = FP.Asin(t2) * FP.Rad2Deg;
+                result.z = FP.Atan2(t1, t0) * FP.Rad2Deg;
+
+                return result * -1;
+            }
+        }
+
+        public static FP Angle(TSQuaternion a, TSQuaternion b) {
+            TSQuaternion aInv = TSQuaternion.Inverse(a);
+            TSQuaternion f = b * aInv;
+
+            FP angle = FP.Acos(f.w) * 2 * FP.Rad2Deg;
+
+            if (angle > 180) {
+                angle = 360 - angle;
+            }
+
+            return angle;
+        }
+
+        /// <summary>
+        /// Quaternions are added.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <returns>The sum of both quaternions.</returns>
+        #region public static JQuaternion Add(JQuaternion quaternion1, JQuaternion quaternion2)
+        public static TSQuaternion Add(TSQuaternion quaternion1, TSQuaternion quaternion2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Add(ref quaternion1, ref quaternion2, out result);
+            return result;
+        }
+
+        public static TSQuaternion LookRotation(TSVector forward) {
+            return CreateFromMatrix(TSMatrix.LookAt(forward, TSVector.up));
+        }
+
+        public static TSQuaternion LookRotation(TSVector forward, TSVector upwards) {
+            return CreateFromMatrix(TSMatrix.LookAt(forward, upwards));
+        }
+
+        public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t) {
+            t = TSMath.Clamp(t, 0, 1);
+
+            FP dot = Dot(from, to);
+
+            if (dot < 0.0f) {
+                to = Multiply(to, -1);
+                dot = -dot;
+            }
+
+            FP halfTheta = FP.Acos(dot);
+
+            return Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta));
+        }
+
+        public static TSQuaternion RotateTowards(TSQuaternion from, TSQuaternion to, FP maxDegreesDelta) {
+            FP dot = Dot(from, to);
+
+            if (dot < 0.0f) {
+                to = Multiply(to, -1);
+                dot = -dot;
+            }
+
+            FP halfTheta = FP.Acos(dot);
+            FP theta = halfTheta * 2;
+
+            maxDegreesDelta *= FP.Deg2Rad;
+
+            if (maxDegreesDelta >= theta) {
+                return to;
+            }
+
+            maxDegreesDelta /= theta;
+
+            return Multiply(Multiply(from, FP.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, FP.Sin(maxDegreesDelta * halfTheta)), 1 / FP.Sin(halfTheta));
+        }
+
+        public static TSQuaternion Euler(FP x, FP y, FP z) {
+            x *= FP.Deg2Rad;
+            y *= FP.Deg2Rad;
+            z *= FP.Deg2Rad;
+
+            TSQuaternion rotation;
+            TSQuaternion.CreateFromYawPitchRoll(y, x, z, out rotation);
+
+            return rotation;
+        }
+
+        public static TSQuaternion Euler(TSVector eulerAngles) {
+            return Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+        }
+
+        public static TSQuaternion AngleAxis(FP angle, TSVector axis) {
+            axis = axis * FP.Deg2Rad;
+            axis.Normalize();
+
+            FP halfAngle = angle * FP.Deg2Rad * FP.Half;
+
+            TSQuaternion rotation;
+            FP sin = FP.Sin(halfAngle);
+
+            rotation.x = axis.x * sin;
+            rotation.y = axis.y * sin;
+            rotation.z = axis.z * sin;
+            rotation.w = FP.Cos(halfAngle);
+
+            return rotation;
+        }
+
+        public static void CreateFromYawPitchRoll(FP yaw, FP pitch, FP roll, out TSQuaternion result)
+        {
+            FP num9 = roll * FP.Half;
+            FP num6 = FP.Sin(num9);
+            FP num5 = FP.Cos(num9);
+            FP num8 = pitch * FP.Half;
+            FP num4 = FP.Sin(num8);
+            FP num3 = FP.Cos(num8);
+            FP num7 = yaw * FP.Half;
+            FP num2 = FP.Sin(num7);
+            FP num = FP.Cos(num7);
+            result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
+            result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
+            result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
+            result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
+        }
+
+        /// <summary>
+        /// Quaternions are added.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <param name="result">The sum of both quaternions.</param>
+        public static void Add(ref TSQuaternion quaternion1, ref TSQuaternion quaternion2, out TSQuaternion result)
+        {
+            result.x = quaternion1.x + quaternion2.x;
+            result.y = quaternion1.y + quaternion2.y;
+            result.z = quaternion1.z + quaternion2.z;
+            result.w = quaternion1.w + quaternion2.w;
+        }
+        #endregion
+
+        public static TSQuaternion Conjugate(TSQuaternion value)
+        {
+            TSQuaternion quaternion;
+            quaternion.x = -value.x;
+            quaternion.y = -value.y;
+            quaternion.z = -value.z;
+            quaternion.w = value.w;
+            return quaternion;
+        }
+
+        public static FP Dot(TSQuaternion a, TSQuaternion b) {
+            return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
+        }
+
+        public static TSQuaternion Inverse(TSQuaternion rotation) {
+            FP invNorm = FP.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));
+            return TSQuaternion.Multiply(TSQuaternion.Conjugate(rotation), invNorm);
+        }
+
+        public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector) {
+            TSVector w = TSVector.Cross(fromVector, toVector);
+            TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
+            q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
+            q.Normalize();
+
+            return q;
+        }
+
+        public static TSQuaternion Lerp(TSQuaternion a, TSQuaternion b, FP t) {
+            t = TSMath.Clamp(t, FP.Zero, FP.One);
+
+            return LerpUnclamped(a, b, t);
+        }
+
+        public static TSQuaternion LerpUnclamped(TSQuaternion a, TSQuaternion b, FP t) {
+            TSQuaternion result = TSQuaternion.Multiply(a, (1 - t)) + TSQuaternion.Multiply(b, t);
+            result.Normalize();
+
+            return result;
+        }
+
+        /// <summary>
+        /// Quaternions are subtracted.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <returns>The difference of both quaternions.</returns>
+        #region public static JQuaternion Subtract(JQuaternion quaternion1, JQuaternion quaternion2)
+        public static TSQuaternion Subtract(TSQuaternion quaternion1, TSQuaternion quaternion2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Subtract(ref quaternion1, ref quaternion2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Quaternions are subtracted.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <param name="result">The difference of both quaternions.</param>
+        public static void Subtract(ref TSQuaternion quaternion1, ref TSQuaternion quaternion2, out TSQuaternion result)
+        {
+            result.x = quaternion1.x - quaternion2.x;
+            result.y = quaternion1.y - quaternion2.y;
+            result.z = quaternion1.z - quaternion2.z;
+            result.w = quaternion1.w - quaternion2.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiply two quaternions.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <returns>The product of both quaternions.</returns>
+        #region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
+        public static TSQuaternion Multiply(TSQuaternion quaternion1, TSQuaternion quaternion2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Multiply(ref quaternion1, ref quaternion2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Multiply two quaternions.
+        /// </summary>
+        /// <param name="quaternion1">The first quaternion.</param>
+        /// <param name="quaternion2">The second quaternion.</param>
+        /// <param name="result">The product of both quaternions.</param>
+        public static void Multiply(ref TSQuaternion quaternion1, ref TSQuaternion quaternion2, out TSQuaternion result)
+        {
+            FP x = quaternion1.x;
+            FP y = quaternion1.y;
+            FP z = quaternion1.z;
+            FP w = quaternion1.w;
+            FP num4 = quaternion2.x;
+            FP num3 = quaternion2.y;
+            FP num2 = quaternion2.z;
+            FP num = quaternion2.w;
+            FP num12 = (y * num2) - (z * num3);
+            FP num11 = (z * num4) - (x * num2);
+            FP num10 = (x * num3) - (y * num4);
+            FP num9 = ((x * num4) + (y * num3)) + (z * num2);
+            result.x = ((x * num) + (num4 * w)) + num12;
+            result.y = ((y * num) + (num3 * w)) + num11;
+            result.z = ((z * num) + (num2 * w)) + num10;
+            result.w = (w * num) - num9;
+        }
+        #endregion
+
+        /// <summary>
+        /// Scale a quaternion
+        /// </summary>
+        /// <param name="quaternion1">The quaternion to scale.</param>
+        /// <param name="scaleFactor">Scale factor.</param>
+        /// <returns>The scaled quaternion.</returns>
+        #region public static JQuaternion Multiply(JQuaternion quaternion1, FP scaleFactor)
+        public static TSQuaternion Multiply(TSQuaternion quaternion1, FP scaleFactor)
+        {
+            TSQuaternion result;
+            TSQuaternion.Multiply(ref quaternion1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Scale a quaternion
+        /// </summary>
+        /// <param name="quaternion1">The quaternion to scale.</param>
+        /// <param name="scaleFactor">Scale factor.</param>
+        /// <param name="result">The scaled quaternion.</param>
+        public static void Multiply(ref TSQuaternion quaternion1, FP scaleFactor, out TSQuaternion result)
+        {
+            result.x = quaternion1.x * scaleFactor;
+            result.y = quaternion1.y * scaleFactor;
+            result.z = quaternion1.z * scaleFactor;
+            result.w = quaternion1.w * scaleFactor;
+        }
+        #endregion
+
+        /// <summary>
+        /// Sets the length of the quaternion to one.
+        /// </summary>
+        #region public void Normalize()
+        public void Normalize()
+        {
+            FP num2 = (((this.x * this.x) + (this.y * this.y)) + (this.z * this.z)) + (this.w * this.w);
+            FP num = 1 / (FP.Sqrt(num2));
+            this.x *= num;
+            this.y *= num;
+            this.z *= num;
+            this.w *= num;
+        }
+        #endregion
+
+        /// <summary>
+        /// Creates a quaternion from a matrix.
+        /// </summary>
+        /// <param name="matrix">A matrix representing an orientation.</param>
+        /// <returns>JQuaternion representing an orientation.</returns>
+        #region public static JQuaternion CreateFromMatrix(JMatrix matrix)
+        public static TSQuaternion CreateFromMatrix(TSMatrix matrix)
+        {
+            TSQuaternion result;
+            TSQuaternion.CreateFromMatrix(ref matrix, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Creates a quaternion from a matrix.
+        /// </summary>
+        /// <param name="matrix">A matrix representing an orientation.</param>
+        /// <param name="result">JQuaternion representing an orientation.</param>
+        public static void CreateFromMatrix(ref TSMatrix matrix, out TSQuaternion result)
+        {
+            FP num8 = (matrix.M11 + matrix.M22) + matrix.M33;
+            if (num8 > FP.Zero)
+            {
+                FP num = FP.Sqrt((num8 + FP.One));
+                result.w = num * FP.Half;
+                num = FP.Half / num;
+                result.x = (matrix.M23 - matrix.M32) * num;
+                result.y = (matrix.M31 - matrix.M13) * num;
+                result.z = (matrix.M12 - matrix.M21) * num;
+            }
+            else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
+            {
+                FP num7 = FP.Sqrt((((FP.One + matrix.M11) - matrix.M22) - matrix.M33));
+                FP num4 = FP.Half / num7;
+                result.x = FP.Half * num7;
+                result.y = (matrix.M12 + matrix.M21) * num4;
+                result.z = (matrix.M13 + matrix.M31) * num4;
+                result.w = (matrix.M23 - matrix.M32) * num4;
+            }
+            else if (matrix.M22 > matrix.M33)
+            {
+                FP num6 = FP.Sqrt((((FP.One + matrix.M22) - matrix.M11) - matrix.M33));
+                FP num3 = FP.Half / num6;
+                result.x = (matrix.M21 + matrix.M12) * num3;
+                result.y = FP.Half * num6;
+                result.z = (matrix.M32 + matrix.M23) * num3;
+                result.w = (matrix.M31 - matrix.M13) * num3;
+            }
+            else
+            {
+                FP num5 = FP.Sqrt((((FP.One + matrix.M33) - matrix.M11) - matrix.M22));
+                FP num2 = FP.Half / num5;
+                result.x = (matrix.M31 + matrix.M13) * num2;
+                result.y = (matrix.M32 + matrix.M23) * num2;
+                result.z = FP.Half * num5;
+                result.w = (matrix.M12 - matrix.M21) * num2;
+            }
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiply two quaternions.
+        /// </summary>
+        /// <param name="value1">The first quaternion.</param>
+        /// <param name="value2">The second quaternion.</param>
+        /// <returns>The product of both quaternions.</returns>
+        #region public static FP operator *(JQuaternion value1, JQuaternion value2)
+        public static TSQuaternion operator *(TSQuaternion value1, TSQuaternion value2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Multiply(ref value1, ref value2,out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Add two quaternions.
+        /// </summary>
+        /// <param name="value1">The first quaternion.</param>
+        /// <param name="value2">The second quaternion.</param>
+        /// <returns>The sum of both quaternions.</returns>
+        #region public static FP operator +(JQuaternion value1, JQuaternion value2)
+        public static TSQuaternion operator +(TSQuaternion value1, TSQuaternion value2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Add(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Subtract two quaternions.
+        /// </summary>
+        /// <param name="value1">The first quaternion.</param>
+        /// <param name="value2">The second quaternion.</param>
+        /// <returns>The difference of both quaternions.</returns>
+        #region public static FP operator -(JQuaternion value1, JQuaternion value2)
+        public static TSQuaternion operator -(TSQuaternion value1, TSQuaternion value2)
+        {
+            TSQuaternion result;
+            TSQuaternion.Subtract(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /**
+         *  @brief Rotates a {@link TSVector} by the {@link TSQuanternion}.
+         **/
+        public static TSVector operator *(TSQuaternion quat, TSVector vec) {
+            FP num = quat.x * 2f;
+            FP num2 = quat.y * 2f;
+            FP num3 = quat.z * 2f;
+            FP num4 = quat.x * num;
+            FP num5 = quat.y * num2;
+            FP num6 = quat.z * num3;
+            FP num7 = quat.x * num2;
+            FP num8 = quat.x * num3;
+            FP num9 = quat.y * num3;
+            FP num10 = quat.w * num;
+            FP num11 = quat.w * num2;
+            FP num12 = quat.w * num3;
+
+            TSVector result;
+            result.x = (1f - (num5 + num6)) * vec.x + (num7 - num12) * vec.y + (num8 + num11) * vec.z;
+            result.y = (num7 + num12) * vec.x + (1f - (num4 + num6)) * vec.y + (num9 - num10) * vec.z;
+            result.z = (num8 - num11) * vec.x + (num9 + num10) * vec.y + (1f - (num4 + num5)) * vec.z;
+
+            return result;
+        }
+
+        public override string ToString() {
+            return string.Format("({0:f1}, {1:f1}, {2:f1}, {3:f1})", x.AsFloat(), y.AsFloat(), z.AsFloat(), w.AsFloat());
+        }
+
+    }
+}

TrueSync-Math/Math/TSRandom.cs

@@ -0,0 +1,280 @@
+using System;
+
+namespace TrueSync {
+
+    /**
+     *  @brief Generates random numbers based on a deterministic approach.
+     **/
+    public class TSRandom {
+        // From http://www.codeproject.com/Articles/164087/Random-Number-Generation
+        // Class TSRandom generates random numbers
+        // from a uniform distribution using the Mersenne
+        // Twister algorithm.
+        private const int N = 624;
+        private const int M = 397;
+        private const uint MATRIX_A = 0x9908b0dfU;
+        private const uint UPPER_MASK = 0x80000000U;
+        private const uint LOWER_MASK = 0x7fffffffU;
+        private const int MAX_RAND_INT = 0x7fffffff;
+        private uint[] mag01 = { 0x0U, MATRIX_A };
+        private uint[] mt = new uint[N];
+        private int mti = N + 1;
+
+        /**
+         *  @brief Static instance of {@link TSRandom} with seed 1.
+         **/
+        public static TSRandom instance;
+
+        internal static void Init() {
+            instance = New(1);
+        }
+
+        /**
+         *  @brief Generates a new instance based on a given seed.
+         **/
+        public static TSRandom New(int seed) {
+            TSRandom r = new TSRandom(seed);
+            return r;
+        }
+
+        private TSRandom() {
+            init_genrand((uint)DateTime.Now.Millisecond);
+        }
+
+        private TSRandom(int seed) {
+            init_genrand((uint)seed);
+        }
+
+        private TSRandom(int[] init) {
+            uint[] initArray = new uint[init.Length];
+            for (int i = 0; i < init.Length; ++i)
+                initArray[i] = (uint)init[i];
+            init_by_array(initArray, (uint)initArray.Length);
+        }
+
+        public static int MaxRandomInt { get { return 0x7fffffff; } }
+
+        /**
+         *  @brief Returns a random integer.
+         **/
+        public int Next() {
+            return genrand_int31();
+        }
+
+        /**
+         *  @brief Returns a random integer.
+         **/
+        public static int CallNext() {
+            return instance.Next();
+        }
+
+        /**
+         *  @brief Returns a integer between a min value [inclusive] and a max value [exclusive].
+         **/
+        public int Next(int minValue, int maxValue) {
+            if (minValue > maxValue) {
+                int tmp = maxValue;
+                maxValue = minValue;
+                minValue = tmp;
+            }
+
+            int range = maxValue - minValue;
+
+            return minValue + Next() % range;
+        }
+
+        /**
+         *  @brief Returns a {@link FP} between a min value [inclusive] and a max value [inclusive].
+         **/
+        public FP Next(float minValue, float maxValue) {
+            int minValueInt = (int)(minValue * 1000), maxValueInt = (int)(maxValue * 1000);
+
+            if (minValueInt > maxValueInt) {
+                int tmp = maxValueInt;
+                maxValueInt = minValueInt;
+                minValueInt = tmp;
+            }
+
+            return (FP.Floor((maxValueInt - minValueInt + 1) * NextFP() +
+                minValueInt)) / 1000;
+        }
+
+        /**
+         *  @brief Returns a integer between a min value [inclusive] and a max value [exclusive].
+         **/
+        public static int Range(int minValue, int maxValue) {
+            return instance.Next(minValue, maxValue);
+        }
+
+        /**
+         *  @brief Returns a {@link FP} between a min value [inclusive] and a max value [inclusive].
+         **/
+        public static FP Range(float minValue, float maxValue) {
+            return instance.Next(minValue, maxValue);
+        }
+
+        /**
+         *  @brief Returns a {@link FP} between 0.0 [inclusive] and 1.0 [inclusive].
+         **/
+        public FP NextFP() {
+            return ((FP) Next()) / (MaxRandomInt);
+        }
+
+        /**
+         *  @brief Returns a {@link FP} between 0.0 [inclusive] and 1.0 [inclusive].
+         **/
+        public static FP value {
+            get {
+                return instance.NextFP();
+            }
+        }
+
+        /**
+         *  @brief Returns a random {@link TSVector} representing a point inside a sphere with radius 1.
+         **/
+        public static TSVector insideUnitSphere {
+            get {
+                return new TSVector(value, value, value);
+            }
+        }
+
+        private float NextFloat() {
+            return (float)genrand_real2();
+        }
+
+        private float NextFloat(bool includeOne) {
+            if (includeOne) {
+                return (float)genrand_real1();
+            }
+            return (float)genrand_real2();
+        }
+
+        private float NextFloatPositive() {
+            return (float)genrand_real3();
+        }
+
+        private double NextDouble() {
+            return genrand_real2();
+        }
+
+        private double NextDouble(bool includeOne) {
+            if (includeOne) {
+                return genrand_real1();
+            }
+            return genrand_real2();
+        }
+
+        private double NextDoublePositive() {
+            return genrand_real3();
+        }
+
+        private double Next53BitRes() {
+            return genrand_res53();
+        }
+
+        public void Initialize() {
+            init_genrand((uint)DateTime.Now.Millisecond);
+        }
+
+        public void Initialize(int seed) {
+            init_genrand((uint)seed);
+        }
+
+        public void Initialize(int[] init) {
+            uint[] initArray = new uint[init.Length];
+            for (int i = 0; i < init.Length; ++i)
+                initArray[i] = (uint)init[i];
+            init_by_array(initArray, (uint)initArray.Length);
+        }
+
+        private void init_genrand(uint s) {
+            mt[0] = s & 0xffffffffU;
+            for (mti = 1; mti < N; mti++) {
+                mt[mti] = (uint)(1812433253U * (mt[mti - 1] ^ (mt[mti - 1] >> 30)) + mti);
+                mt[mti] &= 0xffffffffU;
+            }
+        }
+
+        private void init_by_array(uint[] init_key, uint key_length) {
+            int i, j, k;
+            init_genrand(19650218U);
+            i = 1;
+            j = 0;
+            k = (int)(N > key_length ? N : key_length);
+            for (; k > 0; k--) {
+                mt[i] = (uint)((uint)(mt[i] ^ ((mt[i - 1] ^ (mt[i - 1] >> 30)) * 1664525U)) + init_key[j] + j);
+                mt[i] &= 0xffffffffU;
+                i++;
+                j++;
+                if (i >= N) {
+                    mt[0] = mt[N - 1];
+                    i = 1;
+                }
+                if (j >= key_length)
+                    j = 0;
+            }
+            for (k = N - 1; k > 0; k--) {
+                mt[i] = (uint)((uint)(mt[i] ^ ((mt[i - 1] ^ (mt[i - 1] >> 30)) *
+                    1566083941U)) - i);
+                mt[i] &= 0xffffffffU;
+                i++;
+                if (i >= N) {
+                    mt[0] = mt[N - 1];
+                    i = 1;
+                }
+            }
+            mt[0] = 0x80000000U;
+        }
+
+        uint genrand_int32() {
+            uint y;
+            if (mti >= N) {
+                int kk;
+                if (mti == N + 1)
+                    init_genrand(5489U);
+                for (kk = 0; kk < N - M; kk++) {
+                    y = (mt[kk] & UPPER_MASK) | (mt[kk + 1] & LOWER_MASK);
+                    mt[kk] = mt[kk + M] ^ (y >> 1) ^ mag01[y & 0x1U];
+                }
+                for (; kk < N - 1; kk++) {
+                    y = (mt[kk] & UPPER_MASK) | (mt[kk + 1] & LOWER_MASK);
+                    mt[kk] = mt[kk + (M - N)] ^ (y >> 1) ^ mag01[y & 0x1U];
+                }
+                y = (mt[N - 1] & UPPER_MASK) | (mt[0] & LOWER_MASK);
+                mt[N - 1] = mt[M - 1] ^ (y >> 1) ^ mag01[y & 0x1U];
+                mti = 0;
+            }
+            y = mt[mti++];
+            y ^= (y >> 11);
+            y ^= (y << 7) & 0x9d2c5680U;
+            y ^= (y << 15) & 0xefc60000U;
+            y ^= (y >> 18);
+            return y;
+        }
+
+        private int genrand_int31() {
+            return (int)(genrand_int32() >> 1);
+        }
+
+        FP genrand_FP() {
+            return (FP)genrand_int32() * (FP.One / (FP)4294967295);
+        }
+
+        double genrand_real1() {
+            return genrand_int32() * (1.0 / 4294967295.0);
+        }
+        double genrand_real2() {
+            return genrand_int32() * (1.0 / 4294967296.0);
+        }
+
+        double genrand_real3() {
+            return (((double)genrand_int32()) + 0.5) * (1.0 / 4294967296.0);
+        }
+
+        double genrand_res53() {
+            uint a = genrand_int32() >> 5, b = genrand_int32() >> 6;
+            return (a * 67108864.0 + b) * (1.0 / 9007199254740992.0);
+        }
+    }
+
+}

TrueSync-Math/Math/TSVector.cs

@@ -0,0 +1,843 @@
+/* Copyright (C) <2009-2011> <Thorben Linneweber, Jitter Physics>
+* 
+*  This software is provided 'as-is', without any express or implied
+*  warranty.  In no event will the authors be held liable for any damages
+*  arising from the use of this software.
+*
+*  Permission is granted to anyone to use this software for any purpose,
+*  including commercial applications, and to alter it and redistribute it
+*  freely, subject to the following restrictions:
+*
+*  1. The origin of this software must not be misrepresented; you must not
+*      claim that you wrote the original software. If you use this software
+*      in a product, an acknowledgment in the product documentation would be
+*      appreciated but is not required.
+*  2. Altered source versions must be plainly marked as such, and must not be
+*      misrepresented as being the original software.
+*  3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+
+namespace TrueSync
+{
+    /// <summary>
+    /// A vector structure.
+    /// </summary>
+    public struct TSVector
+    {
+
+        private static FP ZeroEpsilonSq = TSMath.Epsilon;
+        internal static TSVector InternalZero;
+        internal static TSVector Arbitrary;
+
+        /// <summary>The X component of the vector.</summary>
+        public FP x;
+        /// <summary>The Y component of the vector.</summary>
+        public FP y;
+        /// <summary>The Z component of the vector.</summary>
+        public FP z;
+
+        #region Static readonly variables
+        /// <summary>
+        /// A vector with components (0,0,0);
+        /// </summary>
+        public static readonly TSVector zero;
+        /// <summary>
+        /// A vector with components (-1,0,0);
+        /// </summary>
+        public static readonly TSVector left;
+        /// <summary>
+        /// A vector with components (1,0,0);
+        /// </summary>
+        public static readonly TSVector right;
+        /// <summary>
+        /// A vector with components (0,1,0);
+        /// </summary>
+        public static readonly TSVector up;
+        /// <summary>
+        /// A vector with components (0,-1,0);
+        /// </summary>
+        public static readonly TSVector down;
+        /// <summary>
+        /// A vector with components (0,0,-1);
+        /// </summary>
+        public static readonly TSVector back;
+        /// <summary>
+        /// A vector with components (0,0,1);
+        /// </summary>
+        public static readonly TSVector forward;
+        /// <summary>
+        /// A vector with components (1,1,1);
+        /// </summary>
+        public static readonly TSVector one;
+        /// <summary>
+        /// A vector with components 
+        /// (FP.MinValue,FP.MinValue,FP.MinValue);
+        /// </summary>
+        public static readonly TSVector MinValue;
+        /// <summary>
+        /// A vector with components 
+        /// (FP.MaxValue,FP.MaxValue,FP.MaxValue);
+        /// </summary>
+        public static readonly TSVector MaxValue;
+        #endregion
+
+        #region Private static constructor
+        static TSVector()
+        {
+            one = new TSVector(1, 1, 1);
+            zero = new TSVector(0, 0, 0);
+            left = new TSVector(-1, 0, 0);
+            right = new TSVector(1, 0, 0);
+            up = new TSVector(0, 1, 0);
+            down = new TSVector(0, -1, 0);
+            back = new TSVector(0, 0, -1);
+            forward = new TSVector(0, 0, 1);
+            MinValue = new TSVector(FP.MinValue);
+            MaxValue = new TSVector(FP.MaxValue);
+            Arbitrary = new TSVector(1, 1, 1);
+            InternalZero = zero;
+        }
+        #endregion
+
+        public static TSVector Abs(TSVector other) {
+            return new TSVector(FP.Abs(other.x), FP.Abs(other.y), FP.Abs(other.z));
+        }
+
+        /// <summary>
+        /// Gets the squared length of the vector.
+        /// </summary>
+        /// <returns>Returns the squared length of the vector.</returns>
+        public FP sqrMagnitude {
+            get { 
+                return (((this.x * this.x) + (this.y * this.y)) + (this.z * this.z));
+            }
+        }
+
+        /// <summary>
+        /// Gets the length of the vector.
+        /// </summary>
+        /// <returns>Returns the length of the vector.</returns>
+        public FP magnitude {
+            get {
+                FP num = ((this.x * this.x) + (this.y * this.y)) + (this.z * this.z);
+                return FP.Sqrt(num);
+            }
+        }
+
+        public static TSVector ClampMagnitude(TSVector vector, FP maxLength) {
+            return Normalize(vector) * maxLength;
+        }
+
+        /// <summary>
+        /// Gets a normalized version of the vector.
+        /// </summary>
+        /// <returns>Returns a normalized version of the vector.</returns>
+        public TSVector normalized {
+            get {
+                TSVector result = new TSVector(this.x, this.y, this.z);
+                result.Normalize();
+
+                return result;
+            }
+        }
+
+        /// <summary>
+        /// Constructor initializing a new instance of the structure
+        /// </summary>
+        /// <param name="x">The X component of the vector.</param>
+        /// <param name="y">The Y component of the vector.</param>
+        /// <param name="z">The Z component of the vector.</param>
+
+        public TSVector(int x,int y,int z)
+		{
+			this.x = (FP)x;
+			this.y = (FP)y;
+			this.z = (FP)z;
+		}
+
+		public TSVector(FP x, FP y, FP z)
+        {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+        }
+
+        /// <summary>
+        /// Multiplies each component of the vector by the same components of the provided vector.
+        /// </summary>
+        public void Scale(TSVector other) {
+            this.x = x * other.x;
+            this.y = y * other.y;
+            this.z = z * other.z;
+        }
+
+        /// <summary>
+        /// Sets all vector component to specific values.
+        /// </summary>
+        /// <param name="x">The X component of the vector.</param>
+        /// <param name="y">The Y component of the vector.</param>
+        /// <param name="z">The Z component of the vector.</param>
+        public void Set(FP x, FP y, FP z)
+        {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+        }
+
+        /// <summary>
+        /// Constructor initializing a new instance of the structure
+        /// </summary>
+        /// <param name="xyz">All components of the vector are set to xyz</param>
+        public TSVector(FP xyz)
+        {
+            this.x = xyz;
+            this.y = xyz;
+            this.z = xyz;
+        }
+
+		public static TSVector Lerp(TSVector from, TSVector to, FP percent) {
+			return from + (to - from) * percent;
+		}
+
+        /// <summary>
+        /// Builds a string from the JVector.
+        /// </summary>
+        /// <returns>A string containing all three components.</returns>
+        #region public override string ToString()
+        public override string ToString() {
+            return string.Format("({0:f1}, {1:f1}, {2:f1})", x.AsFloat(), y.AsFloat(), z.AsFloat());
+        }
+        #endregion
+
+        /// <summary>
+        /// Tests if an object is equal to this vector.
+        /// </summary>
+        /// <param name="obj">The object to test.</param>
+        /// <returns>Returns true if they are euqal, otherwise false.</returns>
+        #region public override bool Equals(object obj)
+        public override bool Equals(object obj)
+        {
+            if (!(obj is TSVector)) return false;
+            TSVector other = (TSVector)obj;
+
+            return (((x == other.x) && (y == other.y)) && (z == other.z));
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies each component of the vector by the same components of the provided vector.
+        /// </summary>
+        public static TSVector Scale(TSVector vecA, TSVector vecB) {
+            TSVector result;
+            result.x = vecA.x * vecB.x;
+            result.y = vecA.y * vecB.y;
+            result.z = vecA.z * vecB.z;
+
+            return result;
+        }
+
+        /// <summary>
+        /// Tests if two JVector are equal.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>Returns true if both values are equal, otherwise false.</returns>
+        #region public static bool operator ==(JVector value1, JVector value2)
+        public static bool operator ==(TSVector value1, TSVector value2)
+        {
+            return (((value1.x == value2.x) && (value1.y == value2.y)) && (value1.z == value2.z));
+        }
+        #endregion
+
+        /// <summary>
+        /// Tests if two JVector are not equal.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>Returns false if both values are equal, otherwise true.</returns>
+        #region public static bool operator !=(JVector value1, JVector value2)
+        public static bool operator !=(TSVector value1, TSVector value2)
+        {
+            if ((value1.x == value2.x) && (value1.y == value2.y))
+            {
+                return (value1.z != value2.z);
+            }
+            return true;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets a vector with the minimum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>A vector with the minimum x,y and z values of both vectors.</returns>
+        #region public static JVector Min(JVector value1, JVector value2)
+
+        public static TSVector Min(TSVector value1, TSVector value2)
+        {
+            TSVector result;
+            TSVector.Min(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Gets a vector with the minimum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <param name="result">A vector with the minimum x,y and z values of both vectors.</param>
+        public static void Min(ref TSVector value1, ref TSVector value2, out TSVector result)
+        {
+            result.x = (value1.x < value2.x) ? value1.x : value2.x;
+            result.y = (value1.y < value2.y) ? value1.y : value2.y;
+            result.z = (value1.z < value2.z) ? value1.z : value2.z;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets a vector with the maximum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>A vector with the maximum x,y and z values of both vectors.</returns>
+        #region public static JVector Max(JVector value1, JVector value2)
+        public static TSVector Max(TSVector value1, TSVector value2)
+        {
+            TSVector result;
+            TSVector.Max(ref value1, ref value2, out result);
+            return result;
+        }
+		
+		public static FP Distance(TSVector v1, TSVector v2) {
+			return FP.Sqrt ((v1.x - v2.x) * (v1.x - v2.x) + (v1.y - v2.y) * (v1.y - v2.y) + (v1.z - v2.z) * (v1.z - v2.z));
+		}
+
+        /// <summary>
+        /// Gets a vector with the maximum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <param name="result">A vector with the maximum x,y and z values of both vectors.</param>
+        public static void Max(ref TSVector value1, ref TSVector value2, out TSVector result)
+        {
+            result.x = (value1.x > value2.x) ? value1.x : value2.x;
+            result.y = (value1.y > value2.y) ? value1.y : value2.y;
+            result.z = (value1.z > value2.z) ? value1.z : value2.z;
+        }
+        #endregion
+
+        /// <summary>
+        /// Sets the length of the vector to zero.
+        /// </summary>
+        #region public void MakeZero()
+        public void MakeZero()
+        {
+            x = FP.Zero;
+            y = FP.Zero;
+            z = FP.Zero;
+        }
+        #endregion
+
+        /// <summary>
+        /// Checks if the length of the vector is zero.
+        /// </summary>
+        /// <returns>Returns true if the vector is zero, otherwise false.</returns>
+        #region public bool IsZero()
+        public bool IsZero()
+        {
+            return (this.sqrMagnitude == FP.Zero);
+        }
+
+        /// <summary>
+        /// Checks if the length of the vector is nearly zero.
+        /// </summary>
+        /// <returns>Returns true if the vector is nearly zero, otherwise false.</returns>
+        public bool IsNearlyZero()
+        {
+            return (this.sqrMagnitude < ZeroEpsilonSq);
+        }
+        #endregion
+
+        /// <summary>
+        /// Transforms a vector by the given matrix.
+        /// </summary>
+        /// <param name="position">The vector to transform.</param>
+        /// <param name="matrix">The transform matrix.</param>
+        /// <returns>The transformed vector.</returns>
+        #region public static JVector Transform(JVector position, JMatrix matrix)
+        public static TSVector Transform(TSVector position, TSMatrix matrix)
+        {
+            TSVector result;
+            TSVector.Transform(ref position, ref matrix, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Transforms a vector by the given matrix.
+        /// </summary>
+        /// <param name="position">The vector to transform.</param>
+        /// <param name="matrix">The transform matrix.</param>
+        /// <param name="result">The transformed vector.</param>
+        public static void Transform(ref TSVector position, ref TSMatrix matrix, out TSVector result)
+        {
+            FP num0 = ((position.x * matrix.M11) + (position.y * matrix.M21)) + (position.z * matrix.M31);
+            FP num1 = ((position.x * matrix.M12) + (position.y * matrix.M22)) + (position.z * matrix.M32);
+            FP num2 = ((position.x * matrix.M13) + (position.y * matrix.M23)) + (position.z * matrix.M33);
+
+            result.x = num0;
+            result.y = num1;
+            result.z = num2;
+        }
+
+        /// <summary>
+        /// Transforms a vector by the transposed of the given Matrix.
+        /// </summary>
+        /// <param name="position">The vector to transform.</param>
+        /// <param name="matrix">The transform matrix.</param>
+        /// <param name="result">The transformed vector.</param>
+        public static void TransposedTransform(ref TSVector position, ref TSMatrix matrix, out TSVector result)
+        {
+            FP num0 = ((position.x * matrix.M11) + (position.y * matrix.M12)) + (position.z * matrix.M13);
+            FP num1 = ((position.x * matrix.M21) + (position.y * matrix.M22)) + (position.z * matrix.M23);
+            FP num2 = ((position.x * matrix.M31) + (position.y * matrix.M32)) + (position.z * matrix.M33);
+
+            result.x = num0;
+            result.y = num1;
+            result.z = num2;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the dot product of two vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <returns>Returns the dot product of both vectors.</returns>
+        #region public static FP Dot(JVector vector1, JVector vector2)
+        public static FP Dot(TSVector vector1, TSVector vector2)
+        {
+            return TSVector.Dot(ref vector1, ref vector2);
+        }
+
+
+        /// <summary>
+        /// Calculates the dot product of both vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <returns>Returns the dot product of both vectors.</returns>
+        public static FP Dot(ref TSVector vector1, ref TSVector vector2)
+        {
+            return ((vector1.x * vector2.x) + (vector1.y * vector2.y)) + (vector1.z * vector2.z);
+        }
+        #endregion
+
+        // Projects a vector onto another vector.
+        public static TSVector Project(TSVector vector, TSVector onNormal)
+        {
+            FP sqrtMag = Dot(onNormal, onNormal);
+            if (sqrtMag < TSMath.Epsilon)
+                return zero;
+            else
+                return onNormal * Dot(vector, onNormal) / sqrtMag;
+        }
+
+        // Projects a vector onto a plane defined by a normal orthogonal to the plane.
+        public static TSVector ProjectOnPlane(TSVector vector, TSVector planeNormal)
+        {
+            return vector - Project(vector, planeNormal);
+        }
+
+
+        // Returns the angle in degrees between /from/ and /to/. This is always the smallest
+        public static FP Angle(TSVector from, TSVector to)
+        {
+            return TSMath.Acos(TSMath.Clamp(Dot(from.normalized, to.normalized), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg;
+        }
+
+        // The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
+        // If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
+        // The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
+        public static FP SignedAngle(TSVector from, TSVector to, TSVector axis)
+        {
+            TSVector fromNorm = from.normalized, toNorm = to.normalized;
+            FP unsignedAngle = TSMath.Acos(TSMath.Clamp(Dot(fromNorm, toNorm), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg;
+            FP sign = TSMath.Sign(Dot(axis, Cross(fromNorm, toNorm)));
+            return unsignedAngle * sign;
+        }
+
+        /// <summary>
+        /// Adds two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The sum of both vectors.</returns>
+        #region public static void Add(JVector value1, JVector value2)
+        public static TSVector Add(TSVector value1, TSVector value2)
+        {
+            TSVector result;
+            TSVector.Add(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Adds to vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <param name="result">The sum of both vectors.</param>
+        public static void Add(ref TSVector value1, ref TSVector value2, out TSVector result)
+        {
+            FP num0 = value1.x + value2.x;
+            FP num1 = value1.y + value2.y;
+            FP num2 = value1.z + value2.z;
+
+            result.x = num0;
+            result.y = num1;
+            result.z = num2;
+        }
+        #endregion
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        public static TSVector Divide(TSVector value1, FP scaleFactor) {
+            TSVector result;
+            TSVector.Divide(ref value1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <param name="result">Returns the scaled vector.</param>
+        public static void Divide(ref TSVector value1, FP scaleFactor, out TSVector result) {
+            result.x = value1.x / scaleFactor;
+            result.y = value1.y / scaleFactor;
+            result.z = value1.z / scaleFactor;
+        }
+
+        /// <summary>
+        /// Subtracts two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The difference of both vectors.</returns>
+        #region public static JVector Subtract(JVector value1, JVector value2)
+        public static TSVector Subtract(TSVector value1, TSVector value2)
+        {
+            TSVector result;
+            TSVector.Subtract(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Subtracts to vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <param name="result">The difference of both vectors.</param>
+        public static void Subtract(ref TSVector value1, ref TSVector value2, out TSVector result)
+        {
+            FP num0 = value1.x - value2.x;
+            FP num1 = value1.y - value2.y;
+            FP num2 = value1.z - value2.z;
+
+            result.x = num0;
+            result.y = num1;
+            result.z = num2;
+        }
+        #endregion
+
+        /// <summary>
+        /// The cross product of two vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <returns>The cross product of both vectors.</returns>
+        #region public static JVector Cross(JVector vector1, JVector vector2)
+        public static TSVector Cross(TSVector vector1, TSVector vector2)
+        {
+            TSVector result;
+            TSVector.Cross(ref vector1, ref vector2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// The cross product of two vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <param name="result">The cross product of both vectors.</param>
+        public static void Cross(ref TSVector vector1, ref TSVector vector2, out TSVector result)
+        {
+            FP num3 = (vector1.y * vector2.z) - (vector1.z * vector2.y);
+            FP num2 = (vector1.z * vector2.x) - (vector1.x * vector2.z);
+            FP num = (vector1.x * vector2.y) - (vector1.y * vector2.x);
+            result.x = num3;
+            result.y = num2;
+            result.z = num;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the hashcode of the vector.
+        /// </summary>
+        /// <returns>Returns the hashcode of the vector.</returns>
+        #region public override int GetHashCode()
+        public override int GetHashCode()
+        {
+            return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode();
+        }
+        #endregion
+
+        /// <summary>
+        /// Inverses the direction of the vector.
+        /// </summary>
+        #region public static JVector Negate(JVector value)
+        public void Negate()
+        {
+            this.x = -this.x;
+            this.y = -this.y;
+            this.z = -this.z;
+        }
+
+        /// <summary>
+        /// Inverses the direction of a vector.
+        /// </summary>
+        /// <param name="value">The vector to inverse.</param>
+        /// <returns>The negated vector.</returns>
+        public static TSVector Negate(TSVector value)
+        {
+            TSVector result;
+            TSVector.Negate(ref value,out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Inverses the direction of a vector.
+        /// </summary>
+        /// <param name="value">The vector to inverse.</param>
+        /// <param name="result">The negated vector.</param>
+        public static void Negate(ref TSVector value, out TSVector result)
+        {
+            FP num0 = -value.x;
+            FP num1 = -value.y;
+            FP num2 = -value.z;
+
+            result.x = num0;
+            result.y = num1;
+            result.z = num2;
+        }
+        #endregion
+
+        /// <summary>
+        /// Normalizes the given vector.
+        /// </summary>
+        /// <param name="value">The vector which should be normalized.</param>
+        /// <returns>A normalized vector.</returns>
+        #region public static JVector Normalize(JVector value)
+        public static TSVector Normalize(TSVector value)
+        {
+            TSVector result;
+            TSVector.Normalize(ref value, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Normalizes this vector.
+        /// </summary>
+        public void Normalize()
+        {
+            FP num2 = ((this.x * this.x) + (this.y * this.y)) + (this.z * this.z);
+            FP num = FP.One / FP.Sqrt(num2);
+            this.x *= num;
+            this.y *= num;
+            this.z *= num;
+        }
+
+        /// <summary>
+        /// Normalizes the given vector.
+        /// </summary>
+        /// <param name="value">The vector which should be normalized.</param>
+        /// <param name="result">A normalized vector.</param>
+        public static void Normalize(ref TSVector value, out TSVector result)
+        {
+            FP num2 = ((value.x * value.x) + (value.y * value.y)) + (value.z * value.z);
+            FP num = FP.One / FP.Sqrt(num2);
+            result.x = value.x * num;
+            result.y = value.y * num;
+            result.z = value.z * num;
+        }
+        #endregion
+
+        #region public static void Swap(ref JVector vector1, ref JVector vector2)
+
+        /// <summary>
+        /// Swaps the components of both vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector to swap with the second.</param>
+        /// <param name="vector2">The second vector to swap with the first.</param>
+        public static void Swap(ref TSVector vector1, ref TSVector vector2)
+        {
+            FP temp;
+
+            temp = vector1.x;
+            vector1.x = vector2.x;
+            vector2.x = temp;
+
+            temp = vector1.y;
+            vector1.y = vector2.y;
+            vector2.y = temp;
+
+            temp = vector1.z;
+            vector1.z = vector2.z;
+            vector2.z = temp;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiply a vector with a factor.
+        /// </summary>
+        /// <param name="value1">The vector to multiply.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the multiplied vector.</returns>
+        #region public static JVector Multiply(JVector value1, FP scaleFactor)
+        public static TSVector Multiply(TSVector value1, FP scaleFactor)
+        {
+            TSVector result;
+            TSVector.Multiply(ref value1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Multiply a vector with a factor.
+        /// </summary>
+        /// <param name="value1">The vector to multiply.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <param name="result">Returns the multiplied vector.</param>
+        public static void Multiply(ref TSVector value1, FP scaleFactor, out TSVector result)
+        {
+            result.x = value1.x * scaleFactor;
+            result.y = value1.y * scaleFactor;
+            result.z = value1.z * scaleFactor;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the cross product of two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>Returns the cross product of both.</returns>
+        #region public static JVector operator %(JVector value1, JVector value2)
+        public static TSVector operator %(TSVector value1, TSVector value2)
+        {
+            TSVector result; TSVector.Cross(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the dot product of two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>Returns the dot product of both.</returns>
+        #region public static FP operator *(JVector value1, JVector value2)
+        public static FP operator *(TSVector value1, TSVector value2)
+        {
+            return TSVector.Dot(ref value1, ref value2);
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies a vector by a scale factor.
+        /// </summary>
+        /// <param name="value1">The vector to scale.</param>
+        /// <param name="value2">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        #region public static JVector operator *(JVector value1, FP value2)
+        public static TSVector operator *(TSVector value1, FP value2)
+        {
+            TSVector result;
+            TSVector.Multiply(ref value1, value2,out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies a vector by a scale factor.
+        /// </summary>
+        /// <param name="value2">The vector to scale.</param>
+        /// <param name="value1">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        #region public static JVector operator *(FP value1, JVector value2)
+        public static TSVector operator *(FP value1, TSVector value2)
+        {
+            TSVector result;
+            TSVector.Multiply(ref value2, value1, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Subtracts two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The difference of both vectors.</returns>
+        #region public static JVector operator -(JVector value1, JVector value2)
+        public static TSVector operator -(TSVector value1, TSVector value2)
+        {
+            TSVector result; TSVector.Subtract(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Adds two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The sum of both vectors.</returns>
+        #region public static JVector operator +(JVector value1, JVector value2)
+        public static TSVector operator +(TSVector value1, TSVector value2)
+        {
+            TSVector result; TSVector.Add(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        public static TSVector operator /(TSVector value1, FP value2) {
+            TSVector result;
+            TSVector.Divide(ref value1, value2, out result);
+            return result;
+        }
+
+        public TSVector2 ToTSVector2() {
+            return new TSVector2(this.x, this.y);
+        }
+
+        public TSVector4 ToTSVector4()
+        {
+            return new TSVector4(this.x, this.y, this.z, FP.One);
+        }
+
+    }
+
+}

TrueSync-Math/Math/TSVector2.cs

@@ -0,0 +1,585 @@
+#region License
+
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors
+ * Alan McGovern
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#endregion License
+
+using System;
+
+namespace TrueSync {
+
+    [Serializable]
+    public struct TSVector2 : IEquatable<TSVector2>
+    {
+#region Private Fields
+
+        private static TSVector2 zeroVector = new TSVector2(0, 0);
+        private static TSVector2 oneVector = new TSVector2(1, 1);
+
+        private static TSVector2 rightVector = new TSVector2(1, 0);
+        private static TSVector2 leftVector = new TSVector2(-1, 0);
+
+        private static TSVector2 upVector = new TSVector2(0, 1);
+        private static TSVector2 downVector = new TSVector2(0, -1);
+
+        #endregion Private Fields
+
+        #region Public Fields
+
+        public FP x;
+        public FP y;
+
+        #endregion Public Fields
+
+#region Properties
+
+        public static TSVector2 zero
+        {
+            get { return zeroVector; }
+        }
+
+        public static TSVector2 one
+        {
+            get { return oneVector; }
+        }
+
+        public static TSVector2 right
+        {
+            get { return rightVector; }
+        }
+
+        public static TSVector2 left {
+            get { return leftVector; }
+        }
+
+        public static TSVector2 up
+        {
+            get { return upVector; }
+        }
+
+        public static TSVector2 down {
+            get { return downVector; }
+        }
+
+        #endregion Properties
+
+        #region Constructors
+
+        /// <summary>
+        /// Constructor foe standard 2D vector.
+        /// </summary>
+        /// <param name="x">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        /// <param name="y">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        public TSVector2(FP x, FP y)
+        {
+            this.x = x;
+            this.y = y;
+        }
+
+        /// <summary>
+        /// Constructor for "square" vector.
+        /// </summary>
+        /// <param name="value">
+        /// A <see cref="System.Single"/>
+        /// </param>
+        public TSVector2(FP value)
+        {
+            x = value;
+            y = value;
+        }
+
+        public void Set(FP x, FP y) {
+            this.x = x;
+            this.y = y;
+        }
+
+        #endregion Constructors
+
+        #region Public Methods
+
+        public static void Reflect(ref TSVector2 vector, ref TSVector2 normal, out TSVector2 result)
+        {
+            FP dot = Dot(vector, normal);
+            result.x = vector.x - ((2f*dot)*normal.x);
+            result.y = vector.y - ((2f*dot)*normal.y);
+        }
+
+        public static TSVector2 Reflect(TSVector2 vector, TSVector2 normal)
+        {
+            TSVector2 result;
+            Reflect(ref vector, ref normal, out result);
+            return result;
+        }
+
+        public static TSVector2 Add(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x += value2.x;
+            value1.y += value2.y;
+            return value1;
+        }
+
+        public static void Add(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = value1.x + value2.x;
+            result.y = value1.y + value2.y;
+        }
+
+        public static TSVector2 Barycentric(TSVector2 value1, TSVector2 value2, TSVector2 value3, FP amount1, FP amount2)
+        {
+            return new TSVector2(
+                TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
+                TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
+        }
+
+        public static void Barycentric(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, FP amount1,
+                                       FP amount2, out TSVector2 result)
+        {
+            result = new TSVector2(
+                TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
+                TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
+        }
+
+        public static TSVector2 CatmullRom(TSVector2 value1, TSVector2 value2, TSVector2 value3, TSVector2 value4, FP amount)
+        {
+            return new TSVector2(
+                TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
+                TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
+        }
+
+        public static void CatmullRom(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, ref TSVector2 value4,
+                                      FP amount, out TSVector2 result)
+        {
+            result = new TSVector2(
+                TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
+                TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
+        }
+
+        public static TSVector2 Clamp(TSVector2 value1, TSVector2 min, TSVector2 max)
+        {
+            return new TSVector2(
+                TSMath.Clamp(value1.x, min.x, max.x),
+                TSMath.Clamp(value1.y, min.y, max.y));
+        }
+
+        public static void Clamp(ref TSVector2 value1, ref TSVector2 min, ref TSVector2 max, out TSVector2 result)
+        {
+            result = new TSVector2(
+                TSMath.Clamp(value1.x, min.x, max.x),
+                TSMath.Clamp(value1.y, min.y, max.y));
+        }
+
+        /// <summary>
+        /// Returns FP precison distanve between two vectors
+        /// </summary>
+        /// <param name="value1">
+        /// A <see cref="TSVector2"/>
+        /// </param>
+        /// <param name="value2">
+        /// A <see cref="TSVector2"/>
+        /// </param>
+        /// <returns>
+        /// A <see cref="System.Single"/>
+        /// </returns>
+        public static FP Distance(TSVector2 value1, TSVector2 value2)
+        {
+            FP result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return (FP) FP.Sqrt(result);
+        }
+
+
+        public static void Distance(ref TSVector2 value1, ref TSVector2 value2, out FP result)
+        {
+            DistanceSquared(ref value1, ref value2, out result);
+            result = (FP) FP.Sqrt(result);
+        }
+
+        public static FP DistanceSquared(TSVector2 value1, TSVector2 value2)
+        {
+            FP result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static void DistanceSquared(ref TSVector2 value1, ref TSVector2 value2, out FP result)
+        {
+            result = (value1.x - value2.x)*(value1.x - value2.x) + (value1.y - value2.y)*(value1.y - value2.y);
+        }
+
+        /// <summary>
+        /// Devide first vector with the secund vector
+        /// </summary>
+        /// <param name="value1">
+        /// A <see cref="TSVector2"/>
+        /// </param>
+        /// <param name="value2">
+        /// A <see cref="TSVector2"/>
+        /// </param>
+        /// <returns>
+        /// A <see cref="TSVector2"/>
+        /// </returns>
+        public static TSVector2 Divide(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x /= value2.x;
+            value1.y /= value2.y;
+            return value1;
+        }
+
+        public static void Divide(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = value1.x/value2.x;
+            result.y = value1.y/value2.y;
+        }
+
+        public static TSVector2 Divide(TSVector2 value1, FP divider)
+        {
+            FP factor = 1/divider;
+            value1.x *= factor;
+            value1.y *= factor;
+            return value1;
+        }
+
+        public static void Divide(ref TSVector2 value1, FP divider, out TSVector2 result)
+        {
+            FP factor = 1/divider;
+            result.x = value1.x*factor;
+            result.y = value1.y*factor;
+        }
+
+        public static FP Dot(TSVector2 value1, TSVector2 value2)
+        {
+            return value1.x*value2.x + value1.y*value2.y;
+        }
+
+        public static void Dot(ref TSVector2 value1, ref TSVector2 value2, out FP result)
+        {
+            result = value1.x*value2.x + value1.y*value2.y;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is TSVector2) ? this == ((TSVector2) obj) : false;
+        }
+
+        public bool Equals(TSVector2 other)
+        {
+            return this == other;
+        }
+
+        public override int GetHashCode()
+        {
+            return (int) (x + y);
+        }
+
+        public static TSVector2 Hermite(TSVector2 value1, TSVector2 tangent1, TSVector2 value2, TSVector2 tangent2, FP amount)
+        {
+            TSVector2 result = new TSVector2();
+            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
+            return result;
+        }
+
+        public static void Hermite(ref TSVector2 value1, ref TSVector2 tangent1, ref TSVector2 value2, ref TSVector2 tangent2,
+                                   FP amount, out TSVector2 result)
+        {
+            result.x = TSMath.Hermite(value1.x, tangent1.x, value2.x, tangent2.x, amount);
+            result.y = TSMath.Hermite(value1.y, tangent1.y, value2.y, tangent2.y, amount);
+        }
+
+        public FP magnitude {
+            get {
+                FP result;
+                DistanceSquared(ref this, ref zeroVector, out result);
+                return FP.Sqrt(result);
+            }
+        }
+
+        public static TSVector2 ClampMagnitude(TSVector2 vector, FP maxLength) {
+            return Normalize(vector) * maxLength;
+        }
+
+        public FP LengthSquared()
+        {
+            FP result;
+            DistanceSquared(ref this, ref zeroVector, out result);
+            return result;
+        }
+
+        public static TSVector2 Lerp(TSVector2 value1, TSVector2 value2, FP amount) {
+            amount = TSMath.Clamp(amount, 0, 1);
+
+            return new TSVector2(
+                TSMath.Lerp(value1.x, value2.x, amount),
+                TSMath.Lerp(value1.y, value2.y, amount));
+        }
+
+        public static TSVector2 LerpUnclamped(TSVector2 value1, TSVector2 value2, FP amount)
+        {
+            return new TSVector2(
+                TSMath.Lerp(value1.x, value2.x, amount),
+                TSMath.Lerp(value1.y, value2.y, amount));
+        }
+
+        public static void LerpUnclamped(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
+        {
+            result = new TSVector2(
+                TSMath.Lerp(value1.x, value2.x, amount),
+                TSMath.Lerp(value1.y, value2.y, amount));
+        }
+
+        public static TSVector2 Max(TSVector2 value1, TSVector2 value2)
+        {
+            return new TSVector2(
+                TSMath.Max(value1.x, value2.x),
+                TSMath.Max(value1.y, value2.y));
+        }
+
+        public static void Max(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = TSMath.Max(value1.x, value2.x);
+            result.y = TSMath.Max(value1.y, value2.y);
+        }
+
+        public static TSVector2 Min(TSVector2 value1, TSVector2 value2)
+        {
+            return new TSVector2(
+                TSMath.Min(value1.x, value2.x),
+                TSMath.Min(value1.y, value2.y));
+        }
+
+        public static void Min(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = TSMath.Min(value1.x, value2.x);
+            result.y = TSMath.Min(value1.y, value2.y);
+        }
+
+        public void Scale(TSVector2 other) {
+            this.x = x * other.x;
+            this.y = y * other.y;
+        }
+
+        public static TSVector2 Scale(TSVector2 value1, TSVector2 value2) {
+            TSVector2 result;
+            result.x = value1.x * value2.x;
+            result.y = value1.y * value2.y;
+
+            return result;
+        }
+
+        public static TSVector2 Multiply(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x *= value2.x;
+            value1.y *= value2.y;
+            return value1;
+        }
+
+        public static TSVector2 Multiply(TSVector2 value1, FP scaleFactor)
+        {
+            value1.x *= scaleFactor;
+            value1.y *= scaleFactor;
+            return value1;
+        }
+
+        public static void Multiply(ref TSVector2 value1, FP scaleFactor, out TSVector2 result)
+        {
+            result.x = value1.x*scaleFactor;
+            result.y = value1.y*scaleFactor;
+        }
+
+        public static void Multiply(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = value1.x*value2.x;
+            result.y = value1.y*value2.y;
+        }
+
+        public static TSVector2 Negate(TSVector2 value)
+        {
+            value.x = -value.x;
+            value.y = -value.y;
+            return value;
+        }
+
+        public static void Negate(ref TSVector2 value, out TSVector2 result)
+        {
+            result.x = -value.x;
+            result.y = -value.y;
+        }
+
+        public void Normalize()
+        {
+            Normalize(ref this, out this);
+        }
+
+        public static TSVector2 Normalize(TSVector2 value)
+        {
+            Normalize(ref value, out value);
+            return value;
+        }
+
+        public TSVector2 normalized {
+            get {
+                TSVector2 result;
+                TSVector2.Normalize(ref this, out result);
+
+                return result;
+            }
+        }
+
+        public static void Normalize(ref TSVector2 value, out TSVector2 result)
+        {
+            FP factor;
+            DistanceSquared(ref value, ref zeroVector, out factor);
+            factor = 1f/(FP) FP.Sqrt(factor);
+            result.x = value.x*factor;
+            result.y = value.y*factor;
+        }
+
+        public static TSVector2 SmoothStep(TSVector2 value1, TSVector2 value2, FP amount)
+        {
+            return new TSVector2(
+                TSMath.SmoothStep(value1.x, value2.x, amount),
+                TSMath.SmoothStep(value1.y, value2.y, amount));
+        }
+
+        public static void SmoothStep(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
+        {
+            result = new TSVector2(
+                TSMath.SmoothStep(value1.x, value2.x, amount),
+                TSMath.SmoothStep(value1.y, value2.y, amount));
+        }
+
+        public static TSVector2 Subtract(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x -= value2.x;
+            value1.y -= value2.y;
+            return value1;
+        }
+
+        public static void Subtract(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
+        {
+            result.x = value1.x - value2.x;
+            result.y = value1.y - value2.y;
+        }
+
+        public static FP Angle(TSVector2 a, TSVector2 b) {
+            return FP.Acos(a.normalized * b.normalized) * FP.Rad2Deg;
+        }
+
+        public TSVector ToTSVector() {
+            return new TSVector(this.x, this.y, 0);
+        }
+
+        public override string ToString() {
+            return string.Format("({0:f1}, {1:f1})", x.AsFloat(), y.AsFloat());
+        }
+
+        #endregion Public Methods
+
+#region Operators
+
+        public static TSVector2 operator -(TSVector2 value)
+        {
+            value.x = -value.x;
+            value.y = -value.y;
+            return value;
+        }
+
+
+        public static bool operator ==(TSVector2 value1, TSVector2 value2)
+        {
+            return value1.x == value2.x && value1.y == value2.y;
+        }
+
+
+        public static bool operator !=(TSVector2 value1, TSVector2 value2)
+        {
+            return value1.x != value2.x || value1.y != value2.y;
+        }
+
+
+        public static TSVector2 operator +(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x += value2.x;
+            value1.y += value2.y;
+            return value1;
+        }
+
+
+        public static TSVector2 operator -(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x -= value2.x;
+            value1.y -= value2.y;
+            return value1;
+        }
+
+
+        public static FP operator *(TSVector2 value1, TSVector2 value2)
+        {
+            return TSVector2.Dot(value1, value2);
+        }
+
+
+        public static TSVector2 operator *(TSVector2 value, FP scaleFactor)
+        {
+            value.x *= scaleFactor;
+            value.y *= scaleFactor;
+            return value;
+        }
+
+
+        public static TSVector2 operator *(FP scaleFactor, TSVector2 value)
+        {
+            value.x *= scaleFactor;
+            value.y *= scaleFactor;
+            return value;
+        }
+
+
+        public static TSVector2 operator /(TSVector2 value1, TSVector2 value2)
+        {
+            value1.x /= value2.x;
+            value1.y /= value2.y;
+            return value1;
+        }
+
+
+        public static TSVector2 operator /(TSVector2 value1, FP divider)
+        {
+            FP factor = 1/divider;
+            value1.x *= factor;
+            value1.y *= factor;
+            return value1;
+        }
+
+        #endregion Operators
+    }
+}

TrueSync-Math/Math/TSVector4.cs

@@ -0,0 +1,756 @@
+/* Copyright (C) <2009-2011> <Thorben Linneweber, Jitter Physics>
+* 
+*  This software is provided 'as-is', without any express or implied
+*  warranty.  In no event will the authors be held liable for any damages
+*  arising from the use of this software.
+*
+*  Permission is granted to anyone to use this software for any purpose,
+*  including commercial applications, and to alter it and redistribute it
+*  freely, subject to the following restrictions:
+*
+*  1. The origin of this software must not be misrepresented; you must not
+*      claim that you wrote the original software. If you use this software
+*      in a product, an acknowledgment in the product documentation would be
+*      appreciated but is not required.
+*  2. Altered source versions must be plainly marked as such, and must not be
+*      misrepresented as being the original software.
+*  3. This notice may not be removed or altered from any source distribution. 
+*/
+
+using System;
+
+namespace TrueSync
+{
+    /// <summary>
+    /// A vector structure.
+    /// </summary>
+    [Serializable]
+    public struct TSVector4
+    {
+
+        private static FP ZeroEpsilonSq = TSMath.Epsilon;
+        internal static TSVector4 InternalZero;
+
+        /// <summary>The X component of the vector.</summary>
+        public FP x;
+        /// <summary>The Y component of the vector.</summary>
+        public FP y;
+        /// <summary>The Z component of the vector.</summary>
+        public FP z;
+        /// <summary>The W component of the vector.</summary>
+        public FP w;
+
+        #region Static readonly variables
+        /// <summary>
+        /// A vector with components (0,0,0,0);
+        /// </summary>
+        public static readonly TSVector4 zero;
+        /// <summary>
+        /// A vector with components (1,1,1,1);
+        /// </summary>
+        public static readonly TSVector4 one;
+        /// <summary>
+        /// A vector with components 
+        /// (FP.MinValue,FP.MinValue,FP.MinValue);
+        /// </summary>
+        public static readonly TSVector4 MinValue;
+        /// <summary>
+        /// A vector with components 
+        /// (FP.MaxValue,FP.MaxValue,FP.MaxValue);
+        /// </summary>
+        public static readonly TSVector4 MaxValue;
+        #endregion
+
+        #region Private static constructor
+        static TSVector4()
+        {
+            one = new TSVector4(1, 1, 1, 1);
+            zero = new TSVector4(0, 0, 0, 0);
+            MinValue = new TSVector4(FP.MinValue);
+            MaxValue = new TSVector4(FP.MaxValue);
+            InternalZero = zero;
+        }
+        #endregion
+
+        public static TSVector4 Abs(TSVector4 other)
+        {
+            return new TSVector4(FP.Abs(other.x), FP.Abs(other.y), FP.Abs(other.z), FP.Abs(other.z));
+        }
+
+        /// <summary>
+        /// Gets the squared length of the vector.
+        /// </summary>
+        /// <returns>Returns the squared length of the vector.</returns>
+        public FP sqrMagnitude
+        {
+            get
+            {
+                return (((this.x * this.x) + (this.y * this.y)) + (this.z * this.z) + (this.w * this.w));
+            }
+        }
+
+        /// <summary>
+        /// Gets the length of the vector.
+        /// </summary>
+        /// <returns>Returns the length of the vector.</returns>
+        public FP magnitude
+        {
+            get
+            {
+                FP num = sqrMagnitude;
+                return FP.Sqrt(num);
+            }
+        }
+
+        public static TSVector4 ClampMagnitude(TSVector4 vector, FP maxLength)
+        {
+            return Normalize(vector) * maxLength;
+        }
+
+        /// <summary>
+        /// Gets a normalized version of the vector.
+        /// </summary>
+        /// <returns>Returns a normalized version of the vector.</returns>
+        public TSVector4 normalized
+        {
+            get
+            {
+                TSVector4 result = new TSVector4(this.x, this.y, this.z, this.w);
+                result.Normalize();
+
+                return result;
+            }
+        }
+
+        /// <summary>
+        /// Constructor initializing a new instance of the structure
+        /// </summary>
+        /// <param name="x">The X component of the vector.</param>
+        /// <param name="y">The Y component of the vector.</param>
+        /// <param name="z">The Z component of the vector.</param>
+        /// <param name="w">The W component of the vector.</param>
+        public TSVector4(int x, int y, int z, int w)
+        {
+            this.x = (FP)x;
+            this.y = (FP)y;
+            this.z = (FP)z;
+            this.w = (FP)w;
+        }
+
+        public TSVector4(FP x, FP y, FP z, FP w)
+        {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.w = w;
+        }
+
+        /// <summary>
+        /// Multiplies each component of the vector by the same components of the provided vector.
+        /// </summary>
+        public void Scale(TSVector4 other)
+        {
+            this.x = x * other.x;
+            this.y = y * other.y;
+            this.z = z * other.z;
+            this.w = w * other.w;
+        }
+
+        /// <summary>
+        /// Sets all vector component to specific values.
+        /// </summary>
+        /// <param name="x">The X component of the vector.</param>
+        /// <param name="y">The Y component of the vector.</param>
+        /// <param name="z">The Z component of the vector.</param>
+        /// <param name="w">The W component of the vector.</param>
+        public void Set(FP x, FP y, FP z, FP w)
+        {
+            this.x = x;
+            this.y = y;
+            this.z = z;
+            this.w = w;
+        }
+
+        /// <summary>
+        /// Constructor initializing a new instance of the structure
+        /// </summary>
+        /// <param name="xyz">All components of the vector are set to xyz</param>
+        public TSVector4(FP xyzw)
+        {
+            this.x = xyzw;
+            this.y = xyzw;
+            this.z = xyzw;
+            this.w = xyzw;
+        }
+
+        public static TSVector4 Lerp(TSVector4 from, TSVector4 to, FP percent)
+        {
+            return from + (to - from) * percent;
+        }
+
+        /// <summary>
+        /// Builds a string from the JVector.
+        /// </summary>
+        /// <returns>A string containing all three components.</returns>
+        #region public override string ToString()
+        public override string ToString()
+        {
+            return string.Format("({0:f1}, {1:f1}, {2:f1}, {3:f1})", x.AsFloat(), y.AsFloat(), z.AsFloat(), w.AsFloat());
+        }
+        #endregion
+
+        /// <summary>
+        /// Tests if an object is equal to this vector.
+        /// </summary>
+        /// <param name="obj">The object to test.</param>
+        /// <returns>Returns true if they are euqal, otherwise false.</returns>
+        #region public override bool Equals(object obj)
+        public override bool Equals(object obj)
+        {
+            if (!(obj is TSVector4)) return false;
+            TSVector4 other = (TSVector4)obj;
+
+            return (((x == other.x) && (y == other.y)) && (z == other.z) && (w == other.w));
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies each component of the vector by the same components of the provided vector.
+        /// </summary>
+        public static TSVector4 Scale(TSVector4 vecA, TSVector4 vecB)
+        {
+            TSVector4 result;
+            result.x = vecA.x * vecB.x;
+            result.y = vecA.y * vecB.y;
+            result.z = vecA.z * vecB.z;
+            result.w = vecA.w * vecB.w;
+
+            return result;
+        }
+
+        /// <summary>
+        /// Tests if two JVector are equal.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>Returns true if both values are equal, otherwise false.</returns>
+        #region public static bool operator ==(JVector value1, JVector value2)
+        public static bool operator ==(TSVector4 value1, TSVector4 value2)
+        {
+            return (((value1.x == value2.x) && (value1.y == value2.y)) && (value1.z == value2.z) && (value1.w == value2.w));
+        }
+        #endregion
+
+        /// <summary>
+        /// Tests if two JVector are not equal.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>Returns false if both values are equal, otherwise true.</returns>
+        #region public static bool operator !=(JVector value1, JVector value2)
+        public static bool operator !=(TSVector4 value1, TSVector4 value2)
+        {
+            if ((value1.x == value2.x) && (value1.y == value2.y) && (value1.z == value2.z))
+            {
+                return (value1.w != value2.w);
+            }
+            return true;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets a vector with the minimum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>A vector with the minimum x,y and z values of both vectors.</returns>
+        #region public static JVector Min(JVector value1, JVector value2)
+
+        public static TSVector4 Min(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result;
+            TSVector4.Min(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Gets a vector with the minimum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <param name="result">A vector with the minimum x,y and z values of both vectors.</param>
+        public static void Min(ref TSVector4 value1, ref TSVector4 value2, out TSVector4 result)
+        {
+            result.x = (value1.x < value2.x) ? value1.x : value2.x;
+            result.y = (value1.y < value2.y) ? value1.y : value2.y;
+            result.z = (value1.z < value2.z) ? value1.z : value2.z;
+            result.w = (value1.w < value2.w) ? value1.w : value2.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets a vector with the maximum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <returns>A vector with the maximum x,y and z values of both vectors.</returns>
+        #region public static JVector Max(JVector value1, JVector value2)
+        public static TSVector4 Max(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result;
+            TSVector4.Max(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static FP Distance(TSVector4 v1, TSVector4 v2)
+        {
+            return FP.Sqrt((v1.x - v2.x) * (v1.x - v2.x) + (v1.y - v2.y) * (v1.y - v2.y) + (v1.z - v2.z) * (v1.z - v2.z) + (v1.w - v2.w) * (v1.w - v2.w));
+        }
+
+        /// <summary>
+        /// Gets a vector with the maximum x,y and z values of both vectors.
+        /// </summary>
+        /// <param name="value1">The first value.</param>
+        /// <param name="value2">The second value.</param>
+        /// <param name="result">A vector with the maximum x,y and z values of both vectors.</param>
+        public static void Max(ref TSVector4 value1, ref TSVector4 value2, out TSVector4 result)
+        {
+            result.x = (value1.x > value2.x) ? value1.x : value2.x;
+            result.y = (value1.y > value2.y) ? value1.y : value2.y;
+            result.z = (value1.z > value2.z) ? value1.z : value2.z;
+            result.w = (value1.w > value2.w) ? value1.w : value2.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Sets the length of the vector to zero.
+        /// </summary>
+        #region public void MakeZero()
+        public void MakeZero()
+        {
+            x = FP.Zero;
+            y = FP.Zero;
+            z = FP.Zero;
+            w = FP.Zero;
+        }
+        #endregion
+
+        /// <summary>
+        /// Checks if the length of the vector is zero.
+        /// </summary>
+        /// <returns>Returns true if the vector is zero, otherwise false.</returns>
+        #region public bool IsZero()
+        public bool IsZero()
+        {
+            return (this.sqrMagnitude == FP.Zero);
+        }
+
+        /// <summary>
+        /// Checks if the length of the vector is nearly zero.
+        /// </summary>
+        /// <returns>Returns true if the vector is nearly zero, otherwise false.</returns>
+        public bool IsNearlyZero()
+        {
+            return (this.sqrMagnitude < ZeroEpsilonSq);
+        }
+        #endregion
+
+        /// <summary>
+        /// Transforms a vector by the given matrix.
+        /// </summary>
+        /// <param name="position">The vector to transform.</param>
+        /// <param name="matrix">The transform matrix.</param>
+        /// <returns>The transformed vector.</returns>
+        #region public static JVector Transform(JVector position, JMatrix matrix)
+        public static TSVector4 Transform(TSVector4 position, TSMatrix4x4 matrix)
+        {
+            TSVector4 result;
+            TSVector4.Transform(ref position, ref matrix, out result);
+            return result;
+        }
+
+        public static TSVector4 Transform(TSVector position, TSMatrix4x4 matrix)
+        {
+            TSVector4 result;
+            TSVector4.Transform(ref position, ref matrix, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Transforms a vector by the given matrix.
+        /// </summary>
+        /// <param name="vector">The vector to transform.</param>
+        /// <param name="matrix">The transform matrix.</param>
+        /// <param name="result">The transformed vector.</param>
+        public static void Transform(ref TSVector vector, ref TSMatrix4x4 matrix, out TSVector4 result)
+        {
+            result.x = vector.x * matrix.M11 + vector.y * matrix.M12 + vector.z * matrix.M13 + matrix.M14;
+            result.y = vector.x * matrix.M21 + vector.y * matrix.M22 + vector.z * matrix.M23 + matrix.M24;
+            result.z = vector.x * matrix.M31 + vector.y * matrix.M32 + vector.z * matrix.M33 + matrix.M34;
+            result.w = vector.x * matrix.M41 + vector.y * matrix.M42 + vector.z * matrix.M43 + matrix.M44;
+        }
+
+        public static void Transform(ref TSVector4 vector, ref TSMatrix4x4 matrix, out TSVector4 result)
+        {
+            result.x = vector.x * matrix.M11 + vector.y * matrix.M12 + vector.z * matrix.M13 + vector.w * matrix.M14;
+            result.y = vector.x * matrix.M21 + vector.y * matrix.M22 + vector.z * matrix.M23 + vector.w * matrix.M24;
+            result.z = vector.x * matrix.M31 + vector.y * matrix.M32 + vector.z * matrix.M33 + vector.w * matrix.M34;
+            result.w = vector.x * matrix.M41 + vector.y * matrix.M42 + vector.z * matrix.M43 + vector.w * matrix.M44;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the dot product of two vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <returns>Returns the dot product of both vectors.</returns>
+        #region public static FP Dot(JVector vector1, JVector vector2)
+        public static FP Dot(TSVector4 vector1, TSVector4 vector2)
+        {
+            return TSVector4.Dot(ref vector1, ref vector2);
+        }
+
+
+        /// <summary>
+        /// Calculates the dot product of both vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector.</param>
+        /// <param name="vector2">The second vector.</param>
+        /// <returns>Returns the dot product of both vectors.</returns>
+        public static FP Dot(ref TSVector4 vector1, ref TSVector4 vector2)
+        {
+            return ((vector1.x * vector2.x) + (vector1.y * vector2.y)) + (vector1.z * vector2.z) + (vector1.w * vector2.w);
+        }
+        #endregion
+
+        /// <summary>
+        /// Adds two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The sum of both vectors.</returns>
+        #region public static void Add(JVector value1, JVector value2)
+        public static TSVector4 Add(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result;
+            TSVector4.Add(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Adds to vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <param name="result">The sum of both vectors.</param>
+        public static void Add(ref TSVector4 value1, ref TSVector4 value2, out TSVector4 result)
+        {
+            result.x = value1.x + value2.x;
+            result.y = value1.y + value2.y;
+            result.z = value1.z + value2.z;
+            result.w = value1.w + value2.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        public static TSVector4 Divide(TSVector4 value1, FP scaleFactor)
+        {
+            TSVector4 result;
+            TSVector4.Divide(ref value1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <param name="result">Returns the scaled vector.</param>
+        public static void Divide(ref TSVector4 value1, FP scaleFactor, out TSVector4 result)
+        {
+            result.x = value1.x / scaleFactor;
+            result.y = value1.y / scaleFactor;
+            result.z = value1.z / scaleFactor;
+            result.w = value1.w / scaleFactor;
+        }
+
+        /// <summary>
+        /// Subtracts two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The difference of both vectors.</returns>
+        #region public static JVector Subtract(JVector value1, JVector value2)
+        public static TSVector4 Subtract(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result;
+            TSVector4.Subtract(ref value1, ref value2, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Subtracts to vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <param name="result">The difference of both vectors.</param>
+        public static void Subtract(ref TSVector4 value1, ref TSVector4 value2, out TSVector4 result)
+        {
+            result.x = value1.x - value2.x;
+            result.y = value1.y - value2.y;
+            result.z = value1.z - value2.z;
+            result.w = value1.w - value2.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Gets the hashcode of the vector.
+        /// </summary>
+        /// <returns>Returns the hashcode of the vector.</returns>
+        #region public override int GetHashCode()
+        public override int GetHashCode()
+        {
+            return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode();
+        }
+        #endregion
+
+        /// <summary>
+        /// Inverses the direction of the vector.
+        /// </summary>
+        #region public static JVector Negate(JVector value)
+        public void Negate()
+        {
+            this.x = -this.x;
+            this.y = -this.y;
+            this.z = -this.z;
+            this.w = -this.w;
+        }
+
+        /// <summary>
+        /// Inverses the direction of a vector.
+        /// </summary>
+        /// <param name="value">The vector to inverse.</param>
+        /// <returns>The negated vector.</returns>
+        public static TSVector4 Negate(TSVector4 value)
+        {
+            TSVector4 result;
+            TSVector4.Negate(ref value, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Inverses the direction of a vector.
+        /// </summary>
+        /// <param name="value">The vector to inverse.</param>
+        /// <param name="result">The negated vector.</param>
+        public static void Negate(ref TSVector4 value, out TSVector4 result)
+        {
+            result.x = -value.x;
+            result.y = -value.y;
+            result.z = -value.z;
+            result.w = -value.w;
+        }
+        #endregion
+
+        /// <summary>
+        /// Normalizes the given vector.
+        /// </summary>
+        /// <param name="value">The vector which should be normalized.</param>
+        /// <returns>A normalized vector.</returns>
+        #region public static JVector Normalize(JVector value)
+        public static TSVector4 Normalize(TSVector4 value)
+        {
+            TSVector4 result;
+            TSVector4.Normalize(ref value, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Normalizes this vector.
+        /// </summary>
+        public void Normalize()
+        {
+            FP num2 = ((this.x * this.x) + (this.y * this.y)) + (this.z * this.z) + (this.w * this.w);
+            FP num = FP.One / FP.Sqrt(num2);
+            this.x *= num;
+            this.y *= num;
+            this.z *= num;
+            this.w *= num;
+        }
+
+        /// <summary>
+        /// Normalizes the given vector.
+        /// </summary>
+        /// <param name="value">The vector which should be normalized.</param>
+        /// <param name="result">A normalized vector.</param>
+        public static void Normalize(ref TSVector4 value, out TSVector4 result)
+        {
+            FP num2 = ((value.x * value.x) + (value.y * value.y)) + (value.z * value.z) + (value.w * value.w);
+            FP num = FP.One / FP.Sqrt(num2);
+            result.x = value.x * num;
+            result.y = value.y * num;
+            result.z = value.z * num;
+            result.w = value.w * num;
+        }
+        #endregion
+
+        #region public static void Swap(ref JVector vector1, ref JVector vector2)
+
+        /// <summary>
+        /// Swaps the components of both vectors.
+        /// </summary>
+        /// <param name="vector1">The first vector to swap with the second.</param>
+        /// <param name="vector2">The second vector to swap with the first.</param>
+        public static void Swap(ref TSVector4 vector1, ref TSVector4 vector2)
+        {
+            FP temp;
+
+            temp = vector1.x;
+            vector1.x = vector2.x;
+            vector2.x = temp;
+
+            temp = vector1.y;
+            vector1.y = vector2.y;
+            vector2.y = temp;
+
+            temp = vector1.z;
+            vector1.z = vector2.z;
+            vector2.z = temp;
+
+            temp = vector1.w;
+            vector1.w = vector2.w;
+            vector2.w = temp;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiply a vector with a factor.
+        /// </summary>
+        /// <param name="value1">The vector to multiply.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the multiplied vector.</returns>
+        #region public static JVector Multiply(JVector value1, FP scaleFactor)
+        public static TSVector4 Multiply(TSVector4 value1, FP scaleFactor)
+        {
+            TSVector4 result;
+            TSVector4.Multiply(ref value1, scaleFactor, out result);
+            return result;
+        }
+
+        /// <summary>
+        /// Multiply a vector with a factor.
+        /// </summary>
+        /// <param name="value1">The vector to multiply.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <param name="result">Returns the multiplied vector.</param>
+        public static void Multiply(ref TSVector4 value1, FP scaleFactor, out TSVector4 result)
+        {
+            result.x = value1.x * scaleFactor;
+            result.y = value1.y * scaleFactor;
+            result.z = value1.z * scaleFactor;
+            result.w = value1.w * scaleFactor;
+        }
+        #endregion
+
+        /// <summary>
+        /// Calculates the dot product of two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>Returns the dot product of both.</returns>
+        #region public static FP operator *(JVector value1, JVector value2)
+        public static FP operator *(TSVector4 value1, TSVector4 value2)
+        {
+            return TSVector4.Dot(ref value1, ref value2);
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies a vector by a scale factor.
+        /// </summary>
+        /// <param name="value1">The vector to scale.</param>
+        /// <param name="value2">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        #region public static JVector operator *(JVector value1, FP value2)
+        public static TSVector4 operator *(TSVector4 value1, FP value2)
+        {
+            TSVector4 result;
+            TSVector4.Multiply(ref value1, value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Multiplies a vector by a scale factor.
+        /// </summary>
+        /// <param name="value2">The vector to scale.</param>
+        /// <param name="value1">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        #region public static JVector operator *(FP value1, JVector value2)
+        public static TSVector4 operator *(FP value1, TSVector4 value2)
+        {
+            TSVector4 result;
+            TSVector4.Multiply(ref value2, value1, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Subtracts two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The difference of both vectors.</returns>
+        #region public static JVector operator -(JVector value1, JVector value2)
+        public static TSVector4 operator -(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result; TSVector4.Subtract(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Adds two vectors.
+        /// </summary>
+        /// <param name="value1">The first vector.</param>
+        /// <param name="value2">The second vector.</param>
+        /// <returns>The sum of both vectors.</returns>
+        #region public static JVector operator +(JVector value1, JVector value2)
+        public static TSVector4 operator +(TSVector4 value1, TSVector4 value2)
+        {
+            TSVector4 result; TSVector4.Add(ref value1, ref value2, out result);
+            return result;
+        }
+        #endregion
+
+        /// <summary>
+        /// Divides a vector by a factor.
+        /// </summary>
+        /// <param name="value1">The vector to divide.</param>
+        /// <param name="scaleFactor">The scale factor.</param>
+        /// <returns>Returns the scaled vector.</returns>
+        public static TSVector4 operator /(TSVector4 value1, FP value2)
+        {
+            TSVector4 result;
+            TSVector4.Divide(ref value1, value2, out result);
+            return result;
+        }
+
+        public TSVector2 ToTSVector2()
+        {
+            return new TSVector2(this.x, this.y);
+        }
+
+        public TSVector ToTSVector()
+        {
+            return new TSVector(this.x, this.y, this.z);
+        }
+    }
+
+}

TrueSync-Math/Properties/AssemblyInfo.cs

@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// 有关程序集的一般信息由以下
+// 控制。更改这些特性值可修改
+// 与程序集关联的信息。
+[assembly: AssemblyTitle("TrueSync-Math")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("TrueSync-Math")]
+[assembly: AssemblyCopyright("Copyright ©  2021")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// 将 ComVisible 设置为 false 会使此程序集中的类型
+//对 COM 组件不可见。如果需要从 COM 访问此程序集中的类型
+//请将此类型的 ComVisible 特性设置为 true。
+[assembly: ComVisible(false)]
+
+// 如果此项目向 COM 公开，则下列 GUID 用于类型库的 ID
+[assembly: Guid("7f9d0971-ecea-4e83-97a1-888839852d10")]
+
+// 程序集的版本信息由下列四个值组成: 
+//
+//      主版本
+//      次版本
+//      生成号
+//      修订号
+//
+// 可以指定所有值，也可以使用以下所示的 "*" 预置版本号和修订号
+//通过使用 "*"，如下所示:
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]

TrueSync-Math/TrueSync-Math.csproj

@@ -0,0 +1,49 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{7F9D0971-ECEA-4E83-97A1-888839852D10}</ProjectGuid>
+    <OutputType>Library</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>TrueSync_Math</RootNamespace>
+    <AssemblyName>TrueSync-Math</AssemblyName>
+    <TargetFrameworkVersion>v4.7.1</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+    <Deterministic>true</Deterministic>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Compile Include="Math\Fix64.cs" />
+    <Compile Include="Math\Fix64AcosLut.cs" />
+    <Compile Include="Math\Fix64SinLut.cs" />
+    <Compile Include="Math\Fix64TanLut.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Math\TSMath.cs" />
+    <Compile Include="Math\TSMatrix.cs" />
+    <Compile Include="Math\TSMatrix4x4.cs" />
+    <Compile Include="Math\TSQuaternion.cs" />
+    <Compile Include="Math\TSRandom.cs" />
+    <Compile Include="Math\TSVector.cs" />
+    <Compile Include="Math\TSVector2.cs" />
+    <Compile Include="Math\TSVector4.cs" />
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+</Project>