Redstone1024
/
TrueSync-Math
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
_Redstone_c_ 2021-02-13 18:31:48 +08:00
commit 1e4c2a0a9b
16 changed files with 84148 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

362
.gitignore vendored Normal file
View File

@ -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

25
TrueSync-Math.sln Normal file
View File

@ -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

1019
TrueSync-Math/Math/Fix64.cs Normal file
View File

File diff suppressed because it is too large Load Diff

25742
TrueSync-Math/Math/Fix64AcosLut.cs Normal file
View File

File diff suppressed because it is too large Load Diff

25742
TrueSync-Math/Math/Fix64SinLut.cs Normal file
View File

File diff suppressed because it is too large Load Diff

25742
TrueSync-Math/Math/Fix64TanLut.cs Normal file
View File

File diff suppressed because it is too large Load Diff

533
TrueSync-Math/Math/TSMath.cs Normal file
View File

@ -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;
}
}
}

706
TrueSync-Math/Math/TSMatrix.cs Normal file
View File

@ -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);
}
}
}

1206
TrueSync-Math/Math/TSMatrix4x4.cs Normal file
View File

File diff suppressed because it is too large Load Diff

522
TrueSync-Math/Math/TSQuaternion.cs Normal file
View File

@ -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());
}
}
}

280
TrueSync-Math/Math/TSRandom.cs Normal file
View File

@ -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);
}
}
}

843
TrueSync-Math/Math/TSVector.cs Normal file
View File

@ -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);
}
}
}

585
TrueSync-Math/Math/TSVector2.cs Normal file
View File

@ -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
}
}

756
TrueSync-Math/Math/TSVector4.cs Normal file
View File

@ -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);
}
}
}

36
TrueSync-Math/Properties/AssemblyInfo.cs Normal file
View File

@ -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")]

49
TrueSync-Math/TrueSync-Math.csproj Normal file
View File

@ -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>