1 /* This file is part of the Spring engine (GPL v2 or later), see LICENSE.html */
2
3 #ifdef USE_VALGRIND
4 #include <valgrind/valgrind.h>
5 #endif
6
7 #include "System/myMath.h"
8 #include "System/Exceptions.h"
9 #include "System/Sync/FPUCheck.h"
10 #include "System/Log/ILog.h"
11 #include "Sim/Units/Scripts/CobInstance.h" // for TAANG2RAD (ugh)
12
13 float2 CMyMath::headingToVectorTable[NUM_HEADINGS];
14
Init()15 void CMyMath::Init()
16 {
17 good_fpu_init();
18
19 for (int a = 0; a < NUM_HEADINGS; ++a) {
20 float ang = (a - (NUM_HEADINGS / 2)) * 2 * PI / NUM_HEADINGS;
21 float2 v;
22 v.x = math::sin(ang);
23 v.y = math::cos(ang);
24 headingToVectorTable[a] = v;
25 }
26
27 unsigned checksum = 0;
28 for (int a = 0; a < NUM_HEADINGS; ++a) {
29 checksum = 33 * checksum + *(unsigned*) &headingToVectorTable[a].x;
30 checksum *= 33;
31 checksum = 33 * checksum + *(unsigned*) &headingToVectorTable[a].y;
32 }
33
34 #ifdef USE_VALGRIND
35 if (RUNNING_ON_VALGRIND) {
36 // Valgrind doesn't allow us setting the FPU, so syncing is impossible
37 LOG_L(L_WARNING, "Valgrind detected sync checking disabled!");
38 return;
39 }
40 #endif
41
42 #ifdef STREFLOP_H
43 if (checksum != HEADING_CHECKSUM) {
44 throw unsupported_error(
45 "Invalid headingToVectorTable checksum. Most likely"
46 " your streflop library was not compiled with the correct"
47 " options, or you are not using streflop at all.");
48 }
49 #endif
50 }
51
52
53
GetVectorFromHAndPExact(const short int heading,const short int pitch)54 float3 GetVectorFromHAndPExact(const short int heading, const short int pitch)
55 {
56 float3 ret;
57 float h = heading * TAANG2RAD;
58 float p = pitch * TAANG2RAD;
59 ret.x = math::sin(h) * math::cos(p);
60 ret.y = math::sin(p);
61 ret.z = math::cos(h) * math::cos(p);
62 return ret;
63 }
64
LinePointDist(const float3 l1,const float3 l2,const float3 p)65 float LinePointDist(const float3 l1, const float3 l2, const float3 p)
66 {
67 float3 dir(l2 - l1);
68 float length = dir.Length();
69 if (length == 0)
70 length = 0.1f;
71 dir /= length;
72
73 float a = (p - l1).dot(dir);
74 if (a < 0) a = 0;
75 if (a > length) a = length;
76
77 float3 p2 = p - dir * a;
78 return p2.distance(l1);
79 }
80
81 /**
82 * @brief calculate closest point on linepiece from l1 to l2
83 * Note, this clamps the returned point to a position between l1 and l2.
84 */
ClosestPointOnLine(const float3 l1,const float3 l2,const float3 p)85 float3 ClosestPointOnLine(const float3 l1, const float3 l2, const float3 p)
86 {
87 float3 dir(l2-l1);
88 float3 pdir(p-l1);
89 float length = dir.Length();
90 if (math::fabs(length) < 1e-4f)
91 return l1;
92 float c = dir.dot(pdir) / length;
93 if (c < 0) c = 0;
94 if (c > length) c = length;
95 return l1 + dir * (c / length);
96 }
97
98
99 /**
100 * How does it works?
101 * We calculate the two intersection points ON the
102 * ray as multiple of `dir` starting from `start`.
103 * So we get 2 scalars, whereupon `near` defines the
104 * new startpoint and `far` defines the new endpoint.
105 *
106 * credits:
107 * http://ompf.org/ray/ray_box.html
108 */
GetMapBoundaryIntersectionPoints(const float3 start,const float3 dir)109 std::pair<float, float> GetMapBoundaryIntersectionPoints(const float3 start, const float3 dir)
110 {
111 const float rcpdirx = (dir.x != 0.0f)? (1.0f / dir.x): 10000.0f;
112 const float rcpdirz = (dir.z != 0.0f)? (1.0f / dir.z): 10000.0f;
113 float l1, l2, far, near;
114
115 const float& mapwidth = float3::maxxpos + 1;
116 const float& mapheight = float3::maxzpos + 1;
117
118 //! x component
119 l1 = ( 0.0f - start.x) * rcpdirx;
120 l2 = (mapwidth - start.x) * rcpdirx;
121 near = std::min(l1, l2);
122 far = std::max(l1, l2);
123
124 //! z component
125 l1 = ( 0.0f - start.z) * rcpdirz;
126 l2 = (mapheight - start.z) * rcpdirz;
127 near = std::max(std::min(l1, l2), near);
128 far = std::min(std::max(l1, l2), far);
129
130 if (far < 0.0f || far < near) {
131 //! outside of boundary
132 near = -1.0f;
133 far = -1.0f;
134 }
135 return std::pair<float, float>(near, far);
136 }
137
138
ClampLineInMap(float3 & start,float3 & end)139 bool ClampLineInMap(float3& start, float3& end)
140 {
141 const float3 dir = end - start;
142 const std::pair<float, float>& interp = GetMapBoundaryIntersectionPoints(start, dir);
143 const float& near = interp.first;
144 const float& far = interp.second;
145
146 if (far < 0.0f) {
147 //! outside of map!
148 start = -OnesVector;
149 end = -OnesVector;
150 return true;
151 }
152
153 if (far < 1.0f || near > 0.0f) {
154 end = start + dir * std::min(far, 1.0f);
155 start = start + dir * std::max(near, 0.0f);
156
157 //! precision of near,far are limited, so better clamp it afterwards
158 end.ClampInMap();
159 start.ClampInMap();
160 return true;
161 }
162 return false;
163 }
164
165
ClampRayInMap(const float3 start,float3 & end)166 bool ClampRayInMap(const float3 start, float3& end)
167 {
168 const float3 dir = end - start;
169 std::pair<float, float> interp = GetMapBoundaryIntersectionPoints(start, dir);
170 const float& near = interp.first;
171 const float& far = interp.second;
172
173 if (far < 0.0f) {
174 //! outside of map!
175 end = start;
176 return true;
177 }
178
179 if (far < 1.0f || near > 0.0f) {
180 end = start + dir * std::min(far, 1.0f);
181
182 //! precision of near,far are limited, so better clamp it afterwards
183 end.ClampInMap();
184 return true;
185 }
186 return false;
187 }
188
189
smoothstep(const float edge0,const float edge1,const float value)190 float smoothstep(const float edge0, const float edge1, const float value)
191 {
192 if (value <= edge0) return 0.0f;
193 if (value >= edge1) return 1.0f;
194 const float x = (value - edge0) / (edge1 - edge0);
195 const float t = Clamp(x, 0.0f, 1.0f);
196 return (t * t * (3.0f - 2.0f * t));
197 }
198
smoothstep(const float edge0,const float edge1,float3 vec)199 float3 smoothstep(const float edge0, const float edge1, float3 vec)
200 {
201 vec.x = smoothstep(edge0, edge1, vec.x);
202 vec.y = smoothstep(edge0, edge1, vec.y);
203 vec.z = smoothstep(edge0, edge1, vec.z);
204 return vec;
205 }
206
linearstep(const float edge0,const float edge1,const float value)207 float linearstep(const float edge0, const float edge1, const float value)
208 {
209 if (value <= edge0) return 0.0f;
210 if (value >= edge1) return 1.0f;
211 const float x = (value - edge0) / (edge1 - edge0);
212 const float t = Clamp(x, 0.0f, 1.0f);
213 return t;
214 }
215
216
hs2rgb(float h,float s)217 float3 hs2rgb(float h, float s)
218 {
219 // FIXME? ignores saturation completely
220 s = 1.0f;
221
222 const float invSat = 1.0f - s;
223
224 if (h > 0.5f) { h += 0.1f; }
225 if (h > 1.0f) { h -= 1.0f; }
226
227 float3 col(invSat / 2.0f, invSat / 2.0f, invSat / 2.0f);
228
229 if (h < (1.0f / 6.0f)) {
230 col.x += s;
231 col.y += s * (h * 6.0f);
232 } else if (h < (1.0f / 3.0f)) {
233 col.y += s;
234 col.x += s * ((1.0f / 3.0f - h) * 6.0f);
235 } else if (h < (1.0f / 2.0f)) {
236 col.y += s;
237 col.z += s * ((h - (1.0f / 3.0f)) * 6.0f);
238 } else if (h < (2.0f / 3.0f)) {
239 col.z += s;
240 col.y += s * ((2.0f / 3.0f - h) * 6.0f);
241 } else if (h < (5.0f / 6.0f)) {
242 col.z += s;
243 col.x += s * ((h - (2.0f / 3.0f)) * 6.0f);
244 } else {
245 col.x += s;
246 col.z += s * ((3.0f / 3.0f - h) * 6.0f);
247 }
248
249 return col;
250 }
251
252