1 /*
2 Copyright (c) 2013-2017 Timur Gafarov 
3 
4 Boost Software License - Version 1.0 - August 17th, 2003
5 
6 Permission is hereby granted, free of charge, to any person or organization
7 obtaining a copy of the software and accompanying documentation covered by
8 this license (the "Software") to use, reproduce, display, distribute,
9 execute, and transmit the Software, and to prepare derivative works of the
10 Software, and to permit third-parties to whom the Software is furnished to
11 do so, all subject to the following:
12 
13 The copyright notices in the Software and this entire statement, including
14 the above license grant, this restriction and the following disclaimer,
15 must be included in all copies of the Software, in whole or in part, and
16 all derivative works of the Software, unless such copies or derivative
17 works are solely in the form of machine-executable object code generated by
18 a source language processor.
19 
20 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
21 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
22 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
23 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
24 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
25 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
26 DEALINGS IN THE SOFTWARE.
27 */
28 
29 module dmech.solver;
30 
31 import std.math;
32 import std.algorithm;
33 
34 import dlib.math.vector;
35 import dlib.math.utils;
36 
37 import dmech.rigidbody;
38 import dmech.contact;
39 
40 void prepareContact(Contact* c)
41 {
42     if (!c.shape1.active || !c.shape2.active ||
43         !c.shape1.solve || !c.shape2.solve)
44         return;
45 
46     RigidBody body1 = c.body1;
47     RigidBody body2 = c.body2;
48     
49     Vector3f r1 = c.body1RelPoint;
50     Vector3f r2 = c.body2RelPoint;
51     
52     Vector3f relativeVelocity = Vector3f(0.0f, 0.0f, 0.0f);
53     relativeVelocity += body1.linearVelocity + cross(body1.angularVelocity, r1);
54     relativeVelocity -= body2.linearVelocity + cross(body2.angularVelocity, r2);
55     c.initialVelocityProjection = dot(relativeVelocity, c.normal);
56 
57     c.n1 = c.normal;
58     c.w1 = c.normal.cross(r1);
59     c.n2 = -c.normal;
60     c.w2 = -c.normal.cross(r2);
61 
62     c.effectiveMass = 
63         body1.invMass + 
64         body2.invMass +
65         dot(c.w1 * body1.invInertiaTensor, c.w1) +
66         dot(c.w2 * body2.invInertiaTensor, c.w2);
67 }
68 
69 void solveContact(Contact* c, double dt)
70 {
71     if (!c.shape1.active || !c.shape2.active ||
72         !c.shape1.solve || !c.shape2.solve)
73         return;
74 
75     RigidBody body1 = c.body1;
76     RigidBody body2 = c.body2;
77     
78     Vector3f r1 = c.body1RelPoint;
79     Vector3f r2 = c.body2RelPoint;
80 
81     Vector3f relativeVelocity = Vector3f(0.0f, 0.0f, 0.0f);
82     relativeVelocity += body1.linearVelocity + cross(body1.angularVelocity, r1);
83     relativeVelocity -= body2.linearVelocity + cross(body2.angularVelocity, r2);
84     float velocityProjection = dot(relativeVelocity, c.normal);
85 
86     // Check if the bodies are already moving apart
87     if (velocityProjection > 0.0f)
88         return;
89 
90     float bounce = (body1.bounce + body2.bounce) * 0.5f;
91     float damping = 0.9f;
92     float C = max(0, -bounce * c.initialVelocityProjection - damping);
93 
94     float bias = 0.01f;
95 
96     // Velocity-based position correction
97   /*
98     float allowedPenetration = 0.01f;
99     float biasFactor = 0.3f; // 0.1 to 0.3
100     bias = biasFactor * (1.0f / dt) * max(0.0f, c.penetration - allowedPenetration);
101   */
102     float a = velocityProjection;
103     float b = c.effectiveMass;
104 
105     float normalImpulse = (C - a + bias) / b;
106 
107     //if (normalImpulse < 0.0f)
108     //    normalImpulse = 0.0f;
109 
110     // Friction
111     float mu = (body1.friction + body2.friction) * 0.5f;
112     Vector3f fVec = Vector3f(0.0f, 0.0f, 0.0f);
113 
114     Vector3f tn1 = c.fdir1;
115     Vector3f tw1 = c.fdir1.cross(r1);
116     Vector3f tn2 = -c.fdir1;
117     Vector3f tw2 = -c.fdir1.cross(r2);
118     float ta = dot(relativeVelocity, c.fdir1);
119     float tb = dot(tn1, tn1 * body1.invMass)
120              + dot(tw1, tw1 * body1.invInertiaTensor)
121              + dot(tn2, tn2 * body2.invMass)
122              + dot(tw2, tw2 * body2.invInertiaTensor);
123     float fImpulse1 = -ta / tb;
124     fImpulse1 = clamp(fImpulse1, -normalImpulse * mu, normalImpulse * mu);
125 
126     tn1 = c.fdir2;
127     tw1 = c.fdir2.cross(r1);
128     tn2 = -c.fdir2;
129     tw2 = -c.fdir2.cross(r2);
130     ta = dot(relativeVelocity, c.fdir2);
131     tb = dot(tn1, tn1 * body1.invMass)
132        + dot(tw1, tw1 * body1.invInertiaTensor)
133        + dot(tn2, tn2 * body2.invMass)
134        + dot(tw2, tw2 * body2.invInertiaTensor);
135     float fImpulse2 = -ta / tb;
136     fImpulse2 = clamp(fImpulse2, -normalImpulse * mu, normalImpulse * mu);
137 
138     c.accumulatedfImpulse1 += fImpulse1;
139     c.accumulatedfImpulse2 += fImpulse2;
140 
141     fVec = c.fdir1 * fImpulse1 + c.fdir2 * fImpulse2;
142 
143     Vector3f impulseVec = c.normal * normalImpulse;
144 
145     body1.applyImpulse(+impulseVec, c.point);
146     body2.applyImpulse(-impulseVec, c.point);
147     
148     if (body1.useFriction) body1.applyImpulse(+fVec, c.point);
149     if (body2.useFriction) body2.applyImpulse(-fVec, c.point);
150 }
151 
152 void solvePositionError(Contact* c, uint numContacts)
153 {
154     RigidBody body1 = c.body1;
155     RigidBody body2 = c.body2;
156     
157     Vector3f r1 = c.body1RelPoint;
158     Vector3f r2 = c.body2RelPoint;
159        
160     Vector3f prv = Vector3f(0.0f, 0.0f, 0.0f);
161     prv += body1.pseudoLinearVelocity + cross(body1.pseudoAngularVelocity, r1);
162     prv -= body2.pseudoLinearVelocity + cross(body2.pseudoAngularVelocity, r2);
163     float pvp = dot(prv, c.normal);
164 
165     if (c.penetration <= 0.0f)
166         return;
167     
168     float ERP = (1.0f / numContacts) * 0.94f;
169     float pc = c.penetration * ERP;
170     //c.penetration -= pc;
171     c.penetration = 0.0f;
172     
173     if (pvp >= pc)
174         return;
175 
176     float a = pvp;
177     float impulse = (pc - a) / c.effectiveMass;
178 
179     Vector3f impulseVec = c.normal * impulse;
180    
181     body1.applyPseudoImpulse(+impulseVec, c.point);
182     body2.applyPseudoImpulse(-impulseVec, c.point);
183 }
184