1 files changed, 157 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
new file mode 100644
index 0000000..d4206ea
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
@@ -0,0 +1,157 @@
+/*
+  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
+{
+        public class ConstraintSetting
+        {
+                private float _tau, _damping;
+                public ConstraintSetting()
+                {
+                        _tau = 0.3f;
+                        _damping = 1.0f;
+                }
+                public float Damping
+                {
+                        get { return _damping; }
+                        set { _damping = value; }
+                }
+                public float Tau
+                {
+                        get { return _tau; }
+                        set { _tau = value; }
+                }
+        }
+        public class Point2PointConstraint : TypedConstraint
+        {
+                private JacobianEntry[] _jacobian;
+                private Vector3 _pivotInA, _pivotInB;
+                private ConstraintSetting _setting = new ConstraintSetting();
+                public Point2PointConstraint()
+                {
+                        _jacobian = new JacobianEntry[3];
+                }
+                public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
+                        : base(rbA, rbB)
+                {
+                        _jacobian = new JacobianEntry[3];
+                        _pivotInA = pivotInA;
+                        _pivotInB = pivotInB;
+                }
+                public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
+                        : base(rbA)
+                {
+                        _jacobian = new JacobianEntry[3];
+                        _pivotInA = pivotInA;
+                        _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
+                }
+                public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
+                public Vector3 PivotInA
+                {
+                        set
+                        {
+                                _pivotInA = value;
+                        }
+                }
+                public Vector3 PivotInB
+                {
+                        set
+                        {
+                                _pivotInB = value;
+                        }
+                }
+                public override void BuildJacobian()
+                {
+                        Vector3 normal = new Vector3();
+                        for (int i = 0; i < 3; i++)
+                        {
+                                MathHelper.SetElement(ref normal, i, 1);
+                                _jacobian[i] = new JacobianEntry(
+                                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                        MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
+                                        MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
+                                        normal,
+                                        RigidBodyA.InvInertiaDiagLocal,
+                                        RigidBodyA.InverseMass,
+                                        RigidBodyB.InvInertiaDiagLocal,
+                                        RigidBodyB.InverseMass
+                                );
+                                MathHelper.SetElement(ref normal, i, 0);
+                        }
+                }
+                public override void SolveConstraint(float timeStep)
+                {
+                        Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
+                        Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
+                        Vector3 normal = new Vector3();
+                        for (int i = 0; i < 3; i++)
+                        {
+                                MathHelper.SetElement(ref normal, i, 1);
+                                float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
+                                Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
+                                Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
+                                Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
+                                Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
+                                Vector3 vel = vel1 - vel2;
+                                float rel_vel = Vector3.Dot(normal, vel);
+                                float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
+                                float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
+                                AppliedImpulse += impulse;
+                                Vector3 impulseVector = normal * impulse;
+                                RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
+                                RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
+                                MathHelper.SetElement(ref normal, i, 0);
+                        }
+                }
+        }
+}
+\ No newline at end of file

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs new file mode 100644 index 0000000..d4206ea --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
@@ -0,0 +1,157 @@
	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	public class ConstraintSetting
	30	{
	31	private float _tau, _damping;
	32
	33	public ConstraintSetting()
	34	{
	35	_tau = 0.3f;
	36	_damping = 1.0f;
	37	}
	38
	39	public float Damping
	40	{
	41	get { return _damping; }
	42	set { _damping = value; }
	43	}
	44
	45	public float Tau
	46	{
	47	get { return _tau; }
	48	set { _tau = value; }
	49	}
	50	}
	51
	52	public class Point2PointConstraint : TypedConstraint
	53	{
	54	private JacobianEntry[] _jacobian;
	55	private Vector3 _pivotInA, _pivotInB;
	56
	57	private ConstraintSetting _setting = new ConstraintSetting();
	58
	59	public Point2PointConstraint()
	60	{
	61	_jacobian = new JacobianEntry[3];
	62	}
	63
	64	public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
	65	: base(rbA, rbB)
	66	{
	67	_jacobian = new JacobianEntry[3];
	68
	69	_pivotInA = pivotInA;
	70	_pivotInB = pivotInB;
	71	}
	72
	73	public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
	74	: base(rbA)
	75	{
	76	_jacobian = new JacobianEntry[3];
	77
	78	_pivotInA = pivotInA;
	79	_pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
	80	}
	81
	82	public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
	83
	84	public Vector3 PivotInA
	85	{
	86	set
	87	{
	88	_pivotInA = value;
	89	}
	90	}
	91
	92	public Vector3 PivotInB
	93	{
	94	set
	95	{
	96	_pivotInB = value;
	97	}
	98	}
	99
	100	public override void BuildJacobian()
	101	{
	102	Vector3 normal = new Vector3();
	103
	104	for (int i = 0; i < 3; i++)
	105	{
	106	MathHelper.SetElement(ref normal, i, 1);
	107	_jacobian[i] = new JacobianEntry(
	108	MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
	109	MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
	110	MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
	111	MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
	112	normal,
	113	RigidBodyA.InvInertiaDiagLocal,
	114	RigidBodyA.InverseMass,
	115	RigidBodyB.InvInertiaDiagLocal,
	116	RigidBodyB.InverseMass
	117	);
	118	MathHelper.SetElement(ref normal, i, 0);
	119	}
	120	}
	121
	122	public override void SolveConstraint(float timeStep)
	123	{
	124	Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
	125	Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
	126
	127	Vector3 normal = new Vector3();
	128
	129	for (int i = 0; i < 3; i++)
	130	{
	131	MathHelper.SetElement(ref normal, i, 1);
	132
	133	float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
	134
	135	Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
	136	Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
	137
	138	Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
	139	Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
	140
	141	Vector3 vel = vel1 - vel2;
	142
	143	float rel_vel = Vector3.Dot(normal, vel);
	144	float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
	145
	146	float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
	147	AppliedImpulse += impulse;
	148	Vector3 impulseVector = normal * impulse;
	149
	150	RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
	151	RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
	152
	153	MathHelper.SetElement(ref normal, i, 0);
	154	}
	155	}
	156	}
	157	} \ No newline at end of file