dmech.raycast source code

1 /*
2 Copyright (c) 2014-2018 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.raycast;
30 
31 import dlib.math.vector;
32 import dlib.math.matrix;
33 import dlib.math.utils;
34 
35 import dmech.rigidbody;
36 import dmech.geometry;
37 import dmech.shape;
38 import dmech.jss;
39 
40 struct CastResult
41 {
42     bool fact;
43     Vector3f point;
44     Vector3f normal;
45     float param;
46     RigidBody rbody;
47     ShapeComponent shape;
48 }
49 
50 // TODO: warmstarting
51 
52 bool convexRayCast(
53     ShapeComponent shape,
54     Vector3f rayStart,
55     Vector3f rayDir,
56     float maxCastDistance,
57     out CastResult res)
58 {   
59     float hitParam = 0;
60     Vector3f hitPoint = rayStart;
61     Vector3f hitNormal = Vector3f(0, 0, 0);
62     
63     Vector3f v = hitPoint - shape.supportPointGlobal(Vector3f(0, 0, 0));
64     float vSqrd = v.lengthsqr;
65     
66     Vector3f p = Vector3f(0, 0, 0);
67     Vector3f w = Vector3f(0, 0, 0);
68 
69     float v_dot_w = 0;
70     float v_dot_r = 0;
71     
72     JohnsonSimplexSolver jss;
73     jss.initialize();
74     
75     float eps = EPSILON * 100;
76     
77     uint iterations = 0;
78     uint maxIterations = 1000;
79     
80     bool hit = false;
81     
82     while(true)
83     {
84         if ((vSqrd <= eps * jss.getMaxVertexSqrd) || jss.isFullSimplex)
85         {
86             hit = true;
87             break;
88         }
89         
90         if (hitParam > maxCastDistance)
91             break;
92         
93         if (iterations >= maxIterations)
94             break;
95         
96         p = shape.supportPointGlobal(v);
97         w = hitPoint - p;
98 
99         v_dot_w = dot(v, w);
100         
101         if ( v_dot_w > 0 )
102         {
103             v_dot_r = dot(v, rayDir);
104 
105             if (v_dot_r >= 0)
106                 break;
107 
108             hitParam  = hitParam - v_dot_w / v_dot_r;
109             hitPoint  = rayStart + rayDir * hitParam;
110             hitNormal = v;
111             
112             if (!jss.isEmptySimplex)
113             {
114                 for (byte i = 0; i < 4; ++i)
115                     if (jss.isSimplexPoint(i))
116                         jss.setPoint(i, 
117                             hitPoint - jss.getSupportPointOnB(i), 
118                             hitPoint,  jss.getSupportPointOnB(i));
119                 
120                 jss.updateMaxVertex();
121 
122                 for (byte i = 0; i < 4; ++i)
123                     if (jss.isSimplexPoint(i))
124                         jss.updateEdges(i);
125 
126                 for (byte i = 0; i < 4; ++i)
127                     if (jss.isSimplexPoint(i))
128                         jss.updateDeterminants(i);
129             }
130         }
131         
132         jss.addPoint(w, hitPoint, p);
133 
134         bool reduceOk = jss.reduceSimplex();
135         if (reduceOk)
136             jss.calcClosestPoint(v);
137         else
138             jss.backupCalcClosestPoint(v);
139             
140         vSqrd = v.lengthsqr;
141         ++iterations;
142     }
143     
144     if (hit)
145     {
146         res.fact = true;
147         res.point = hitPoint;
148         res.normal = hitNormal;
149         res.normal.normalize();
150         res.param = hitParam;
151     }
152     
153     return hit;
154 }
155