1 files changed, 188 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
new file mode 100644
index 0000000..6ced783
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -0,0 +1,188 @@
+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+  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;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+namespace XnaDevRu.BulletX.Dynamics
+{
+        /// <summary>
+        /// constraint class used for lateral tyre friction
+        /// </summary>
+        public class Solve2LinearConstraint
+        {
+                private float _tau;
+                private float _damping;
+                public Solve2LinearConstraint(float tau, float damping)
+                {
+                        _tau = tau;
+                        _damping = damping;
+                }
+                // solve unilateral constraint (equality, direct method)
+                public void ResolveUnilateralPairConstraint(
+                                                        RigidBody body1, RigidBody body2,
+                                                        Matrix world2A,
+                                                        Matrix world2B,
+                                                        Vector3 invInertiaADiag,
+                                                        float invMassA,
+                                                        Vector3 linvelA, Vector3 angvelA,
+                                                        Vector3 rel_posA1,
+                                                        Vector3 invInertiaBDiag,
+                                                        float invMassB,
+                                                        Vector3 linvelB, Vector3 angvelB,
+                                                        Vector3 rel_posA2,
+                                                        float depthA, Vector3 normalA,
+                                                        Vector3 rel_posB1, Vector3 rel_posB2,
+                                                        float depthB, Vector3 normalB,
+                                                        out float imp0, out float imp1)
+                {
+                        imp0 = 0;
+                        imp1 = 0;
+                        float len = Math.Abs(normalA.Length()) - 1f;
+                        if (Math.Abs(len) >= float.Epsilon)
+                                return;
+                        BulletDebug.Assert(len < float.Epsilon);
+                        //this jacobian entry could be re-used for all iterations
+                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
+                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
+                        //      btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+                        float massTerm = 1f / (invMassA + invMassB);
+                        // calculate rhs (or error) terms
+                        float dv0 = depthA * _tau * massTerm - vel0 * _damping;
+                        float dv1 = depthB * _tau * massTerm - vel1 * _damping;
+                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
+                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
+                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
+                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
+                }
+                // solving 2x2 lcp problem (inequality, direct solution )
+                public void ResolveBilateralPairConstraint(
+                                                        RigidBody body1, RigidBody body2,
+                                                        Matrix world2A, Matrix world2B,
+                                                        Vector3 invInertiaADiag,
+                                                        float invMassA,
+                                                        Vector3 linvelA, Vector3 angvelA,
+                                                        Vector3 rel_posA1,
+                                                        Vector3 invInertiaBDiag,
+                                                        float invMassB,
+                                                        Vector3 linvelB, Vector3 angvelB,
+                                                        Vector3 rel_posA2,
+                                                        float depthA, Vector3 normalA,
+                                                        Vector3 rel_posB1, Vector3 rel_posB2,
+                                                        float depthB, Vector3 normalB,
+                                                        out float imp0, out float imp1)
+                {
+                        imp0 = 0f;
+                        imp1 = 0f;
+                        float len = Math.Abs(normalA.Length()) - 1f;
+                        if (Math.Abs(len) >= float.Epsilon)
+                                return;
+                        BulletDebug.Assert(len < float.Epsilon);
+                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
+                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
+                        // calculate rhs (or error) terms
+                        float dv0 = depthA * _tau - vel0 * _damping;
+                        float dv1 = depthB * _tau - vel1 * _damping;
+                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
+                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
+                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
+                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
+                        if (imp0 > 0.0f)
+                        {
+                                if (imp1 <= 0.0f)
+                                {
+                                        imp1 = 0f;
+                                        // now imp0>0 imp1<0
+                                        imp0 = dv0 / jacA.Diagonal;
+                                        if (imp0 < 0.0f)
+                                                imp0 = 0f;
+                                }
+                        }
+                        else
+                        {
+                                imp0 = 0f;
+                                imp1 = dv1 / jacB.Diagonal;
+                                if (imp1 <= 0.0f)
+                                {
+                                        imp1 = 0f;
+                                        // now imp0>0 imp1<0
+                                        imp0 = dv0 / jacA.Diagonal;
+                                        if (imp0 > 0.0f)
+                                        {
+                                        }
+                                        else
+                                        {
+                                                imp0 = 0f;
+                                        }
+                                }
+                        }
+                }
+                //public void ResolveAngularConstraint(
+                //                Matrix invInertiaAWS,
+                //                float invMassA,
+                //                Vector3 linvelA, Vector3 angvelA,
+                //                Vector3 rel_posA1,
+                //                Matrix invInertiaBWS,
+                //                float invMassB,
+                //                Vector3 linvelB, Vector3 angvelB,
+                //                Vector3 rel_posA2,
+                //                float depthA, Vector3 normalA,
+                //                Vector3 rel_posB1, Vector3 rel_posB2,
+                //                float depthB, Vector3 normalB,
+                //                out float imp0, out float imp1)
+                //{
+                //    imp0 = 0;
+                //    imp1 = 0;
+                //}
+        }
+}

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs new file mode 100644 index 0000000..6ced783 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -0,0 +1,188 @@
	1	/*
	2	Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
	3	Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
	4
	5	This software is provided 'as-is', without any express or implied
	6	warranty. In no event will the authors be held liable for any damages
	7	arising from the use of this software.
	8
	9	Permission is granted to anyone to use this software for any purpose,
	10	including commercial applications, and to alter it and redistribute it
	11	freely, subject to the following restrictions:
	12
	13	1. The origin of this software must not be misrepresented; you must not
	14	claim that you wrote the original software. If you use this software
	15	in a product, an acknowledgment in the product documentation would be
	16	appreciated but is not required.
	17	2. Altered source versions must be plainly marked as such, and must not be
	18	misrepresented as being the original software.
	19	3. This notice may not be removed or altered from any source distribution.
	20	*/
	21
	22	using System;
	23	using System.Collections.Generic;
	24	using System.Text;
	25	using MonoXnaCompactMaths;
	26
	27	namespace XnaDevRu.BulletX.Dynamics
	28	{
	29	/// <summary>
	30	/// constraint class used for lateral tyre friction
	31	/// </summary>
	32	public class Solve2LinearConstraint
	33	{
	34	private float _tau;
	35	private float _damping;
	36
	37	public Solve2LinearConstraint(float tau, float damping)
	38	{
	39	_tau = tau;
	40	_damping = damping;
	41	}
	42
	43	// solve unilateral constraint (equality, direct method)
	44	public void ResolveUnilateralPairConstraint(
	45	RigidBody body1, RigidBody body2,
	46	Matrix world2A,
	47	Matrix world2B,
	48	Vector3 invInertiaADiag,
	49	float invMassA,
	50	Vector3 linvelA, Vector3 angvelA,
	51	Vector3 rel_posA1,
	52	Vector3 invInertiaBDiag,
	53	float invMassB,
	54	Vector3 linvelB, Vector3 angvelB,
	55	Vector3 rel_posA2,
	56	float depthA, Vector3 normalA,
	57	Vector3 rel_posB1, Vector3 rel_posB2,
	58	float depthB, Vector3 normalB,
	59	out float imp0, out float imp1)
	60	{
	61	imp0 = 0;
	62	imp1 = 0;
	63
	64	float len = Math.Abs(normalA.Length()) - 1f;
	65	if (Math.Abs(len) >= float.Epsilon)
	66	return;
	67
	68	BulletDebug.Assert(len < float.Epsilon);
	69
	70	//this jacobian entry could be re-used for all iterations
	71	JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
	72	invInertiaBDiag, invMassB);
	73	JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
	74	invInertiaBDiag, invMassB);
	75
	76	float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
	77	float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
	78
	79	// btScalar penetrationImpulse = (depthcontactTautimeCorrection) * massTerm;//jacDiagABInv
	80	float massTerm = 1f / (invMassA + invMassB);
	81
	82	// calculate rhs (or error) terms
	83	float dv0 = depthA * _tau * massTerm - vel0 * _damping;
	84	float dv1 = depthB * _tau * massTerm - vel1 * _damping;
	85
	86	float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
	87	float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
	88
	89	imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
	90	imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
	91	}
	92
	93	// solving 2x2 lcp problem (inequality, direct solution )
	94	public void ResolveBilateralPairConstraint(
	95	RigidBody body1, RigidBody body2,
	96	Matrix world2A, Matrix world2B,
	97	Vector3 invInertiaADiag,
	98	float invMassA,
	99	Vector3 linvelA, Vector3 angvelA,
	100	Vector3 rel_posA1,
	101	Vector3 invInertiaBDiag,
	102	float invMassB,
	103	Vector3 linvelB, Vector3 angvelB,
	104	Vector3 rel_posA2,
	105	float depthA, Vector3 normalA,
	106	Vector3 rel_posB1, Vector3 rel_posB2,
	107	float depthB, Vector3 normalB,
	108	out float imp0, out float imp1)
	109	{
	110	imp0 = 0f;
	111	imp1 = 0f;
	112
	113	float len = Math.Abs(normalA.Length()) - 1f;
	114	if (Math.Abs(len) >= float.Epsilon)
	115	return;
	116
	117	BulletDebug.Assert(len < float.Epsilon);
	118
	119	JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
	120	invInertiaBDiag, invMassB);
	121	JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
	122	invInertiaBDiag, invMassB);
	123
	124	float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
	125	float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
	126
	127	// calculate rhs (or error) terms
	128	float dv0 = depthA * _tau - vel0 * _damping;
	129	float dv1 = depthB * _tau - vel1 * _damping;
	130
	131	float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
	132	float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
	133
	134	imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
	135	imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
	136
	137	if (imp0 > 0.0f)
	138	{
	139	if (imp1 <= 0.0f)
	140	{
	141	imp1 = 0f;
	142
	143	// now imp0>0 imp1<0
	144	imp0 = dv0 / jacA.Diagonal;
	145	if (imp0 < 0.0f)
	146	imp0 = 0f;
	147	}
	148	}
	149	else
	150	{
	151	imp0 = 0f;
	152
	153	imp1 = dv1 / jacB.Diagonal;
	154	if (imp1 <= 0.0f)
	155	{
	156	imp1 = 0f;
	157	// now imp0>0 imp1<0
	158	imp0 = dv0 / jacA.Diagonal;
	159	if (imp0 > 0.0f)
	160	{
	161	}
	162	else
	163	{
	164	imp0 = 0f;
	165	}
	166	}
	167	}
	168	}
	169
	170	//public void ResolveAngularConstraint(
	171	// Matrix invInertiaAWS,
	172	// float invMassA,
	173	// Vector3 linvelA, Vector3 angvelA,
	174	// Vector3 rel_posA1,
	175	// Matrix invInertiaBWS,
	176	// float invMassB,
	177	// Vector3 linvelB, Vector3 angvelB,
	178	// Vector3 rel_posA2,
	179	// float depthA, Vector3 normalA,
	180	// Vector3 rel_posB1, Vector3 rel_posB2,
	181	// float depthB, Vector3 normalB,
	182	// out float imp0, out float imp1)
	183	//{
	184	// imp0 = 0;
	185	// imp1 = 0;
	186	//}
	187	}
	188	}