-
Notifications
You must be signed in to change notification settings - Fork 2
/
PointOnPoint.cs
168 lines (133 loc) · 6.18 KB
/
PointOnPoint.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
/* 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.
*/
#region Using Statements
using System;
using System.Collections.Generic;
using JPhysics.Dynamics;
using JPhysics.LinearMath;
using JPhysics.Collision.Shapes;
#endregion
namespace JPhysics.Dynamics.Constraints
{
using LinearMath;
public class PointOnPoint : Constraint
{
private JVector localAnchor1, localAnchor2;
private JVector r1, r2;
private float biasFactor = 0.05f;
private float softness = 0.01f;
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public PointOnPoint(RigidBody body1, RigidBody body2, JVector anchor)
: base(body1, body2)
{
JVector.Subtract(ref anchor, ref body1.position, out localAnchor1);
JVector.Subtract(ref anchor, ref body2.position, out localAnchor2);
JVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
JVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
}
public float AppliedImpulse { get { return accumulatedImpulse; } }
/// <summary>
/// Defines how big the applied impulses can get.
/// </summary>
public float Softness { get { return softness; } set { softness = value; } }
/// <summary>
/// Defines how big the applied impulses can get which correct errors.
/// </summary>
public float BiasFactor { get { return biasFactor; } set { biasFactor = value; } }
float effectiveMass = 0.0f;
float accumulatedImpulse = 0.0f;
float bias;
float softnessOverDt;
JVector[] jacobian = new JVector[4];
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
float deltaLength = dp.Length();
JVector n = p2 - p1;
if (n.LengthSquared() != 0.0f) n.Normalize();
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
float jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
float softnessScalar = accumulatedImpulse * softnessOverDt;
float lambda = -effectiveMass * (jv + bias + softnessScalar);
accumulatedImpulse += lambda;
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += JVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
body2.angularVelocity += JVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
}
public override void DebugDraw(IDebugDrawer drawer)
{
drawer.DrawLine(body1.position, body1.position + r1);
drawer.DrawLine(body2.position, body2.position + r2);
}
}
}