1 /* 2 Copyright (c) 2013 yvt 3 4 This file is part of OpenSpades. 5 6 OpenSpades is free software: you can redistribute it and/or modify 7 it under the terms of the GNU General Public License as published by 8 the Free Software Foundation, either version 3 of the License, or 9 (at your option) any later version. 10 11 OpenSpades is distributed in the hope that it will be useful, 12 but WITHOUT ANY WARRANTY; without even the implied warranty of 13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 GNU General Public License for more details. 15 16 You should have received a copy of the GNU General Public License 17 along with OpenSpades. If not, see <http://www.gnu.org/licenses/>. 18 19 */ 20 21 #include <new> 22 #include <cstdlib> 23 24 #include "Math.h" 25 #include <Core/Debug.h> 26 #include <Core/ThreadLocalStorage.h> 27 28 namespace spades { 29 /* 30 Vector3 Line3::Project(spades::Vector3 v, 31 bool supposeUnbounded) { 32 33 } 34 35 float Line3::GetDistanceTo(spades::Vector3 v, 36 bool supposeUnbounded){ 37 38 } 39 */ 40 41 namespace { 42 std::random_device r_device; 43 std::mt19937_64 global_rng{r_device()}; 44 std::mutex global_rng_mutex; 45 } // namespace 46 47 /** Thread-local random number generator. */ 48 class LocalRNG { 49 public: 50 using result_type = std::uint64_t; 51 LocalRNG()52 LocalRNG() { 53 auto sampleRandom = [] { 54 std::lock_guard<std::mutex> lock{global_rng_mutex}; 55 return global_rng(); 56 }; 57 do { 58 s[0] = sampleRandom(); 59 } while (s[0] == 0); 60 do { 61 s[1] = sampleRandom(); 62 } while (s[1] == 0); 63 } 64 operator ()()65 result_type operator()() { 66 uint64_t x = s[0]; 67 uint64_t y = s[1]; 68 s[0] = y; 69 x ^= x << 23; // a 70 s[1] = x ^ y ^ (x >> 17) ^ (y >> 26); // b, c 71 return s[1] + y; 72 } 73 SampleFloat()74 float SampleFloat() { return std::uniform_real_distribution<float>{}(*this); } 75 SampleInt(T a,T b)76 template <class T = int> inline T SampleInt(T a, T b) { 77 return std::uniform_int_distribution<T>{a, b}(*this); 78 } 79 min()80 static constexpr result_type min() { return 0; } max()81 static constexpr result_type max() { return std::numeric_limits<result_type>::max(); } 82 83 private: 84 std::uint64_t s[2]; 85 }; 86 87 namespace { 88 AutoDeletedThreadLocalStorage<LocalRNG> thread_local_rng; 89 GetThreadLocalRNG()90 LocalRNG &GetThreadLocalRNG() { 91 LocalRNG *rng = thread_local_rng.GetPointer(); 92 if (!rng) { 93 thread_local_rng = rng = new LocalRNG(); 94 } 95 return *rng; 96 } 97 } // namespace 98 SampleRandom()99 std::uint_fast64_t SampleRandom() { return GetThreadLocalRNG()(); } 100 SampleRandomFloat()101 float SampleRandomFloat() { 102 return std::uniform_real_distribution<float>{}(GetThreadLocalRNG()); 103 } 104 SampleRandomInt(T a,T b)105 template <class T> T SampleRandomInt(T a, T b) { 106 return std::uniform_int_distribution<T>{a, b}(GetThreadLocalRNG()); 107 } 108 109 // Note: `uniform_int_distribution` does not accept `char` nor `unsigned char` 110 // (N4659 29.6.1.1 [rand.req.genl]) 111 template short SampleRandomInt(short a, short b); 112 template unsigned short SampleRandomInt(unsigned short a, unsigned short b); 113 template int SampleRandomInt(int a, int b); 114 template unsigned int SampleRandomInt(unsigned int a, unsigned int b); 115 template long SampleRandomInt(long a, long b); 116 template unsigned long SampleRandomInt(unsigned long a, unsigned long b); 117 template long long SampleRandomInt(long long a, long long b); 118 template unsigned long long SampleRandomInt(unsigned long long a, unsigned long long b); 119 Matrix4Multiply(const float a[16],const float b[16],float out[16])120 void Matrix4Multiply(const float a[16], const float b[16], float out[16]) { 121 out[0] = b[0] * a[0] + b[1] * a[4] + b[2] * a[8] + b[3] * a[12]; 122 out[1] = b[0] * a[1] + b[1] * a[5] + b[2] * a[9] + b[3] * a[13]; 123 out[2] = b[0] * a[2] + b[1] * a[6] + b[2] * a[10] + b[3] * a[14]; 124 out[3] = b[0] * a[3] + b[1] * a[7] + b[2] * a[11] + b[3] * a[15]; 125 126 out[4] = b[4] * a[0] + b[5] * a[4] + b[6] * a[8] + b[7] * a[12]; 127 out[5] = b[4] * a[1] + b[5] * a[5] + b[6] * a[9] + b[7] * a[13]; 128 out[6] = b[4] * a[2] + b[5] * a[6] + b[6] * a[10] + b[7] * a[14]; 129 out[7] = b[4] * a[3] + b[5] * a[7] + b[6] * a[11] + b[7] * a[15]; 130 131 out[8] = b[8] * a[0] + b[9] * a[4] + b[10] * a[8] + b[11] * a[12]; 132 out[9] = b[8] * a[1] + b[9] * a[5] + b[10] * a[9] + b[11] * a[13]; 133 out[10] = b[8] * a[2] + b[9] * a[6] + b[10] * a[10] + b[11] * a[14]; 134 out[11] = b[8] * a[3] + b[9] * a[7] + b[10] * a[11] + b[11] * a[15]; 135 136 out[12] = b[12] * a[0] + b[13] * a[4] + b[14] * a[8] + b[15] * a[12]; 137 out[13] = b[12] * a[1] + b[13] * a[5] + b[14] * a[9] + b[15] * a[13]; 138 out[14] = b[12] * a[2] + b[13] * a[6] + b[14] * a[10] + b[15] * a[14]; 139 out[15] = b[12] * a[3] + b[13] * a[7] + b[14] * a[11] + b[15] * a[15]; 140 } 141 Matrix4(float * elms)142 Matrix4::Matrix4(float *elms) { 143 for (int i = 0; i < 16; i++) 144 m[i] = elms[i]; 145 } 146 Matrix4(float m00,float m10,float m20,float m30,float m01,float m11,float m21,float m31,float m02,float m12,float m22,float m32,float m03,float m13,float m23,float m33)147 Matrix4::Matrix4(float m00, float m10, float m20, float m30, float m01, float m11, float m21, 148 float m31, float m02, float m12, float m22, float m32, float m03, float m13, 149 float m23, float m33) { 150 m[0] = m00; 151 m[1] = m10; 152 m[2] = m20; 153 m[3] = m30; 154 m[4] = m01; 155 m[5] = m11; 156 m[6] = m21; 157 m[7] = m31; 158 m[8] = m02; 159 m[9] = m12; 160 m[10] = m22; 161 m[11] = m32; 162 m[12] = m03; 163 m[13] = m13; 164 m[14] = m23; 165 m[15] = m33; 166 } 167 operator *(const spades::Matrix4 & other) const168 Matrix4 Matrix4::operator*(const spades::Matrix4 &other) const { 169 Matrix4 out; 170 Matrix4Multiply(m, other.m, out.m); 171 return Matrix4(out); 172 } 173 operator *(const Matrix4 & mat,const Vector4 & v)174 Vector4 operator*(const Matrix4 &mat, const Vector4 &v) { 175 float x, y, z, w; 176 x = mat.m[0] * v.x; 177 y = mat.m[1] * v.x; 178 z = mat.m[2] * v.x; 179 w = mat.m[3] * v.x; 180 x += mat.m[4] * v.y; 181 y += mat.m[5] * v.y; 182 z += mat.m[6] * v.y; 183 w += mat.m[7] * v.y; 184 x += mat.m[8] * v.z; 185 y += mat.m[9] * v.z; 186 z += mat.m[10] * v.z; 187 w += mat.m[11] * v.z; 188 x += mat.m[12] * v.w; 189 y += mat.m[13] * v.w; 190 z += mat.m[14] * v.w; 191 w += mat.m[15] * v.w; 192 return MakeVector4(x, y, z, w); 193 } 194 operator *(const Matrix4 & mat,const Vector3 & v)195 Vector4 operator*(const Matrix4 &mat, const Vector3 &v) { 196 return mat * MakeVector4(v.x, v.y, v.z, 1.f); 197 } 198 GetOrigin() const199 Vector3 Matrix4::GetOrigin() const { return MakeVector3(m[12], m[13], m[14]); } 200 GetAxis(int axis) const201 Vector3 Matrix4::GetAxis(int axis) const { 202 switch (axis) { 203 case 0: return MakeVector3(m[0], m[1], m[2]); 204 case 1: return MakeVector3(m[4], m[5], m[6]); 205 default: SPAssert(false); 206 case 2: return MakeVector3(m[8], m[9], m[10]); 207 } 208 } 209 FromAxis(spades::Vector3 a1,spades::Vector3 a2,spades::Vector3 a3,spades::Vector3 origin)210 Matrix4 Matrix4::FromAxis(spades::Vector3 a1, spades::Vector3 a2, spades::Vector3 a3, 211 spades::Vector3 origin) { 212 return Matrix4(a1.x, a1.y, a1.z, 0.f, a2.x, a2.y, a2.z, 0.f, a3.x, a3.y, a3.z, 0.f, 213 origin.x, origin.y, origin.z, 1.f); 214 } 215 Identity()216 Matrix4 Matrix4::Identity() { return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); } 217 Translate(spades::Vector3 v)218 Matrix4 Matrix4::Translate(spades::Vector3 v) { return Translate(v.x, v.y, v.z); } 219 Translate(float x,float y,float z)220 Matrix4 Matrix4::Translate(float x, float y, float z) { 221 return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, x, y, z, 1); 222 } 223 Rotate(spades::Vector3 axis,float theta)224 Matrix4 Matrix4::Rotate(spades::Vector3 axis, float theta) { 225 axis = axis.Normalize(); 226 227 float c = cosf(theta), s = sinf(theta); 228 float ic = 1.f - c; 229 float x = axis.x, y = axis.y, z = axis.z; 230 231 return Matrix4(x * x * ic + c, x * y * ic + z * s, x * z * ic - y * s, 0, 232 x * y * ic - z * s, y * y * ic + c, y * z * ic + x * s, 0, 233 x * z * ic + y * s, y * z * ic - x * s, z * z * ic + c, 0, 0, 0, 0, 1); 234 } Scale(float f)235 Matrix4 Matrix4::Scale(float f) { return Scale(f, f, f); } Scale(spades::Vector3 v)236 Matrix4 Matrix4::Scale(spades::Vector3 v) { return Scale(v.x, v.y, v.z); } Scale(float x,float y,float z)237 Matrix4 Matrix4::Scale(float x, float y, float z) { 238 return Matrix4(x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1); 239 } Transposed() const240 Matrix4 Matrix4::Transposed() const { 241 return Matrix4(m[0], m[4], m[8], m[12], m[1], m[5], m[9], m[13], m[2], m[6], m[10], m[14], 242 m[3], m[7], m[11], m[15]); 243 } 244 inverseMatrix4(float * matrix)245 static void inverseMatrix4(float *matrix) { 246 // based on three.js's getInverse, which is in turn based on 247 // http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm 248 249 float n11 = matrix[0], n12 = matrix[4], n13 = matrix[8], n14 = matrix[12]; 250 float n21 = matrix[1], n22 = matrix[5], n23 = matrix[9], n24 = matrix[13]; 251 float n31 = matrix[2], n32 = matrix[6], n33 = matrix[10], n34 = matrix[14]; 252 float n41 = matrix[3], n42 = matrix[7], n43 = matrix[11], n44 = matrix[15]; 253 254 matrix[0] = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - 255 n23 * n32 * n44 + n22 * n33 * n44; 256 matrix[4] = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + 257 n13 * n32 * n44 - n12 * n33 * n44; 258 matrix[8] = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - 259 n13 * n22 * n44 + n12 * n23 * n44; 260 matrix[12] = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + 261 n13 * n22 * n34 - n12 * n23 * n34; 262 matrix[1] = n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + 263 n23 * n31 * n44 - n21 * n33 * n44; 264 matrix[5] = n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - 265 n13 * n31 * n44 + n11 * n33 * n44; 266 matrix[9] = n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + 267 n13 * n21 * n44 - n11 * n23 * n44; 268 matrix[13] = n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - 269 n13 * n21 * n34 + n11 * n23 * n34; 270 matrix[2] = n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - 271 n22 * n31 * n44 + n21 * n32 * n44; 272 matrix[6] = n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + 273 n12 * n31 * n44 - n11 * n32 * n44; 274 matrix[10] = n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - 275 n12 * n21 * n44 + n11 * n22 * n44; 276 matrix[14] = n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + 277 n12 * n21 * n34 - n11 * n22 * n34; 278 matrix[3] = n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + 279 n22 * n31 * n43 - n21 * n32 * n43; 280 matrix[7] = n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - 281 n12 * n31 * n43 + n11 * n32 * n43; 282 matrix[11] = n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + 283 n12 * n21 * n43 - n11 * n22 * n43; 284 matrix[15] = n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - 285 n12 * n21 * n33 + n11 * n22 * n33; 286 287 float det = n11 * matrix[0] + n21 * matrix[4] + n31 * matrix[8] + n41 * matrix[12]; 288 float idet = 1.0f / det; 289 for (int i = 0; i < 16; ++i) { 290 matrix[i] *= idet; 291 } 292 } 293 Inversed() const294 Matrix4 Matrix4::Inversed() const { 295 Matrix4 mm = *this; 296 inverseMatrix4(mm.m); 297 return mm; 298 } 299 InversedFast() const300 Matrix4 Matrix4::InversedFast() const { 301 Matrix4 out = Matrix4::Identity(); 302 Vector3 axis1 = GetAxis(0); 303 Vector3 axis2 = GetAxis(1); 304 Vector3 axis3 = GetAxis(2); 305 Vector3 norm1 = axis1 / axis1.GetPoweredLength(); 306 Vector3 norm2 = axis2 / axis2.GetPoweredLength(); 307 Vector3 norm3 = axis3 / axis3.GetPoweredLength(); 308 out.m[0] = norm1.x; 309 out.m[1] = norm2.x; 310 out.m[2] = norm3.x; 311 out.m[4] = norm1.y; 312 out.m[5] = norm2.y; 313 out.m[6] = norm3.y; 314 out.m[8] = norm1.z; 315 out.m[9] = norm2.z; 316 out.m[10] = norm3.z; 317 318 Vector3 s = (out * GetOrigin()).GetXYZ(); 319 out.m[12] = -s.x; 320 out.m[13] = -s.y; 321 out.m[14] = -s.z; 322 323 return out; 324 } 325 operator OBB3() const326 AABB3::operator OBB3() const { 327 Vector3 siz = max - min; 328 return OBB3( 329 Matrix4(siz.x, 0, 0, 0, 0, siz.y, 0, 0, 0, 0, siz.z, 0, min.x, min.y, min.z, 1)); 330 } 331 RayCast(spades::Vector3 start,spades::Vector3 dir,spades::Vector3 * hitPos)332 bool OBB3::RayCast(spades::Vector3 start, spades::Vector3 dir, spades::Vector3 *hitPos) { 333 Vector3 normX = {m.m[0], m.m[1], m.m[2]}; 334 Vector3 normY = {m.m[4], m.m[5], m.m[6]}; 335 Vector3 normZ = {m.m[8], m.m[9], m.m[10]}; 336 337 // subtract offset 338 Vector3 origin = {m.m[12], m.m[13], m.m[14]}; 339 start -= origin; 340 341 Vector3 end = start + dir; 342 343 float dotX = Vector3::Dot(dir, normX); 344 float dotY = Vector3::Dot(dir, normY); 345 float dotZ = Vector3::Dot(dir, normZ); 346 347 // inside? 348 if (*this && start) { 349 *hitPos = start; 350 return true; 351 } 352 353 // x-plane hit test 354 if (dotX != 0.f) { 355 float startp = Vector3::Dot(start, normX); 356 float endp = Vector3::Dot(end, normX); 357 float boxp = Vector3::Dot(normX, normX); 358 float hit; // 0=start, 1=end 359 if (startp < endp) { 360 hit = startp / (startp - endp); 361 } else { 362 hit = (boxp - startp) / (endp - startp); 363 } 364 if (hit >= 0.f) { 365 *hitPos = start + dir * hit; 366 367 float yd = Vector3::Dot(*hitPos, normY); 368 float zd = Vector3::Dot(*hitPos, normZ); 369 if (yd >= 0 && zd >= 0 && yd <= normY.GetPoweredLength() && 370 zd <= normZ.GetPoweredLength()) { 371 // hit x-plane 372 *hitPos += origin; 373 return true; 374 } 375 } 376 } 377 378 // y-plane hit test 379 if (dotY != 0.f) { 380 float startp = Vector3::Dot(start, normY); 381 float endp = Vector3::Dot(end, normY); 382 float boxp = Vector3::Dot(normY, normY); 383 float hit; // 0=start, 1=end 384 if (startp < endp) { 385 hit = startp / (startp - endp); 386 } else { 387 hit = (boxp - startp) / (endp - startp); 388 } 389 if (hit >= 0.f) { 390 *hitPos = start + dir * hit; 391 392 float xd = Vector3::Dot(*hitPos, normX); 393 float zd = Vector3::Dot(*hitPos, normZ); 394 if (xd >= 0 && zd >= 0 && xd <= normX.GetPoweredLength() && 395 zd <= normZ.GetPoweredLength()) { 396 // hit y-plane 397 *hitPos += origin; 398 return true; 399 } 400 } 401 } 402 403 // z-plane hit test 404 if (dotZ != 0.f) { 405 float startp = Vector3::Dot(start, normZ); 406 float endp = Vector3::Dot(end, normZ); 407 float boxp = Vector3::Dot(normZ, normZ); 408 float hit; // 0=start, 1=end 409 if (startp < endp) { 410 hit = startp / (startp - endp); 411 } else { 412 hit = (boxp - startp) / (endp - startp); 413 } 414 if (hit >= 0.f) { 415 *hitPos = start + dir * hit; 416 417 float xd = Vector3::Dot(*hitPos, normX); 418 float yd = Vector3::Dot(*hitPos, normY); 419 if (xd >= 0 && yd >= 0 && xd <= normX.GetPoweredLength() && 420 yd <= normY.GetPoweredLength()) { 421 // hit z-plane 422 *hitPos += origin; 423 return true; 424 } 425 } 426 } 427 428 return false; 429 } 430 operator &&(const spades::Vector3 & v) const431 bool OBB3::operator&&(const spades::Vector3 &v) const { 432 Matrix4 rmat = m.InversedFast(); 433 Vector3 r = (rmat * v).GetXYZ(); 434 return r.x >= 0.f && r.y >= 0.f && r.z >= 0.f && r.x < 1.f && r.y < 1.f && r.z < 1.f; 435 } 436 GetDistanceTo(const spades::Vector3 & v) const437 float OBB3::GetDistanceTo(const spades::Vector3 &v) const { 438 if (*this && v) { 439 return 0.f; 440 } 441 442 Matrix4 rmat = m.InversedFast(); 443 Vector3 r = (rmat * v).GetXYZ(); 444 Vector3 pt = r; 445 446 // find nearest point 447 r.x = std::max(r.x, 0.f); 448 r.y = std::max(r.y, 0.f); 449 r.z = std::max(r.z, 0.f); 450 r.x = std::min(r.x, 1.f); 451 r.y = std::min(r.y, 1.f); 452 r.z = std::min(r.z, 1.f); 453 454 // compute difference in local space 455 pt -= r; 456 457 // scale to global space 458 float len; 459 len = pt.x * pt.x * m.GetAxis(0).GetPoweredLength(); 460 len += pt.y * pt.y * m.GetAxis(1).GetPoweredLength(); 461 len += pt.z * pt.z * m.GetAxis(2).GetPoweredLength(); 462 463 return len; 464 } 465 GetBoundingAABB() const466 AABB3 OBB3::GetBoundingAABB() const { 467 Vector3 orig = m.GetOrigin(); 468 Vector3 axis1 = m.GetAxis(0); 469 Vector3 axis2 = m.GetAxis(1); 470 Vector3 axis3 = m.GetAxis(2); 471 AABB3 ab(orig.x, orig.y, orig.z, 0, 0, 0); 472 ab += orig + axis1; 473 ab += orig + axis2; 474 ab += orig + axis1 + axis2; 475 ab += orig + axis3; 476 ab += orig + axis3 + axis1; 477 ab += orig + axis3 + axis2; 478 ab += orig + axis3 + axis1 + axis2; 479 return ab; 480 } 481 Replace(const std::string & text,const std::string & before,const std::string & after)482 std::string Replace(const std::string &text, const std::string &before, 483 const std::string &after) { 484 std::string out; 485 size_t pos = 0; 486 while (pos < text.size()) { 487 size_t newPos = text.find(before, pos); 488 if (newPos == std::string::npos) { 489 out.append(text, pos, text.size() - pos); 490 break; 491 } 492 493 out.append(text, pos, newPos - pos); 494 out += after; 495 pos = newPos + before.size(); 496 } 497 return out; 498 } 499 Split(const std::string & str,const std::string & sep)500 std::vector<std::string> Split(const std::string &str, const std::string &sep) { 501 std::vector<std::string> strs; 502 size_t pos = 0; 503 while (pos < str.size()) { 504 size_t newPos = str.find(sep, pos); 505 if (newPos == std::string::npos) { 506 strs.push_back(str.substr(pos)); 507 break; 508 } 509 strs.push_back(str.substr(pos, newPos - pos)); 510 pos = newPos + sep.size(); 511 } 512 return strs; 513 } 514 SplitIntoLines(const std::string & str)515 std::vector<std::string> SplitIntoLines(const std::string &str) { 516 std::vector<std::string> strs; 517 size_t pos = 0; 518 char newLineChar = 0; 519 std::string buf; 520 for (pos = 0; pos < str.size(); pos++) { 521 if (str[pos] == 13 || str[pos] == 10) { 522 if (newLineChar == 0) 523 newLineChar = str[pos]; 524 if (str[pos] != newLineChar) { 525 continue; 526 } 527 strs.push_back(buf); 528 buf.clear(); 529 continue; 530 } 531 buf += str[pos]; 532 } 533 strs.push_back(buf); 534 535 return strs; 536 } 537 EscapeControlCharacters(const std::string & str)538 std::string EscapeControlCharacters(const std::string &str) { 539 std::string out; 540 out.reserve(str.size() * 2); 541 for (size_t i = 0; i < str.size(); i++) { 542 char c = str[i]; 543 if ((c >= 0 && c < 0x20 && c != 10 && c != 13 && c != 9)) { 544 out += '<'; 545 switch (c) { 546 case 0x00: out += "NUL"; break; 547 case 0x01: out += "SOH"; break; 548 case 0x02: out += "STX"; break; 549 case 0x03: out += "ETX"; break; 550 case 0x04: out += "EOT"; break; 551 case 0x05: out += "ENQ"; break; 552 case 0x06: out += "ACK"; break; 553 case 0x07: out += "BEL"; break; 554 case 0x08: out += "BS"; break; 555 case 0x09: out += "TAB"; break; 556 case 0x0a: out += "LF"; break; 557 case 0x0b: out += "VT"; break; 558 case 0x0c: out += "FF"; break; 559 case 0x0d: out += "CR"; break; 560 case 0x0e: out += "SO"; break; 561 case 0x0f: out += "SI"; break; 562 case 0x10: out += "DLE"; break; 563 case 0x11: out += "DC1"; break; 564 case 0x12: out += "DC2"; break; 565 case 0x13: out += "DC3"; break; 566 case 0x14: out += "DC4"; break; 567 case 0x15: out += "NAK"; break; 568 case 0x16: out += "SYN"; break; 569 case 0x17: out += "ETB"; break; 570 case 0x18: out += "CAN"; break; 571 case 0x19: out += "EM"; break; 572 case 0x1a: out += "SUB"; break; 573 case 0x1b: out += "ESC"; break; 574 case 0x1c: out += "FS"; break; 575 case 0x1d: out += "GS"; break; 576 case 0x1e: out += "RS"; break; 577 case 0x1f: out += "US"; break; 578 } 579 out += '>'; 580 } else { 581 out += c; 582 } 583 } 584 return out; 585 } 586 TrimSpaces(const std::string & str)587 std::string TrimSpaces(const std::string &str) { 588 size_t pos = str.find_first_not_of(" \t\n\r"); 589 if (pos == std::string::npos) 590 return std::string(); 591 size_t po2 = str.find_last_not_of(" \t\n\r"); 592 SPAssert(po2 != std::string::npos); 593 SPAssert(po2 >= pos); 594 return str.substr(pos, po2 - pos + 1); 595 } 596 PlaneCullTest(const Plane3 & plane,const AABB3 & box)597 bool PlaneCullTest(const Plane3 &plane, const AABB3 &box) { 598 Vector3 testVertex; 599 // find the vertex with the greatest distance value 600 if (plane.n.x >= 0.f) { 601 if (plane.n.y >= 0.f) { 602 if (plane.n.z >= 0.f) { 603 testVertex = box.max; 604 } else { 605 testVertex = MakeVector3(box.max.x, box.max.y, box.min.z); 606 } 607 } else { 608 if (plane.n.z >= 0.f) { 609 testVertex = MakeVector3(box.max.x, box.min.y, box.max.z); 610 } else { 611 testVertex = MakeVector3(box.max.x, box.min.y, box.min.z); 612 } 613 } 614 } else { 615 if (plane.n.y >= 0.f) { 616 if (plane.n.z >= 0.f) { 617 testVertex = MakeVector3(box.min.x, box.max.y, box.max.z); 618 } else { 619 testVertex = MakeVector3(box.min.x, box.max.y, box.min.z); 620 } 621 } else { 622 if (plane.n.z >= 0.f) { 623 testVertex = MakeVector3(box.min.x, box.min.y, box.max.z); 624 } else { 625 testVertex = box.min; 626 } 627 } 628 } 629 return plane.GetDistanceTo(testVertex) >= 0.f; 630 } 631 EqualsIgnoringCase(const std::string & a,const std::string & b)632 bool EqualsIgnoringCase(const std::string &a, const std::string &b) { 633 if (a.size() != b.size()) 634 return false; 635 if (&a == &b) 636 return true; 637 for (size_t siz = a.size(), i = 0; i < siz; i++) { 638 char x = a[i], y = b[i]; 639 if (tolower(x) != tolower(y)) 640 return false; 641 } 642 return true; 643 } 644 GetCodePointFromUTF8String(const std::string & str,size_t start,size_t * outNumBytes)645 uint32_t GetCodePointFromUTF8String(const std::string &str, size_t start, size_t *outNumBytes) { 646 if (start >= str.size()) { 647 if (outNumBytes) 648 *outNumBytes = 0; 649 return 0; 650 } 651 if ((str[start] & 0x80) == 0) { 652 if (outNumBytes) 653 *outNumBytes = 1; 654 return (uint32_t)str[start]; 655 } 656 657 // WARNING: start <= str.size() - 2 is bad (this leads to security holes) 658 uint32_t ret; 659 if ((str[start] & 0xe0) == 0xc0 && start + 2 <= str.size()) { 660 if (outNumBytes) 661 *outNumBytes = 2; 662 ret = (str[start] & 0x1f) << 6; 663 ret |= (str[start + 1] & 0x3f); 664 return ret; 665 } 666 if ((str[start] & 0xf0) == 0xe0 && start + 3 <= str.size()) { 667 if (outNumBytes) 668 *outNumBytes = 3; 669 ret = (str[start] & 0xf) << 12; 670 ret |= (str[start + 1] & 0x3f) << 6; 671 ret |= (str[start + 2] & 0x3f); 672 return ret; 673 } 674 if ((str[start] & 0xf8) == 0xf0 && start + 4 <= str.size()) { 675 if (outNumBytes) 676 *outNumBytes = 4; 677 ret = (str[start] & 0x7) << 18; 678 ret |= (str[start + 1] & 0x3f) << 12; 679 ret |= (str[start + 2] & 0x3f) << 6; 680 ret |= (str[start + 3] & 0x3f); 681 return ret; 682 } 683 if ((str[start] & 0xfc) == 0xf8 && start + 5 <= str.size()) { 684 if (outNumBytes) 685 *outNumBytes = 5; 686 ret = (str[start] & 0x3) << 24; 687 ret |= (str[start + 1] & 0x3f) << 18; 688 ret |= (str[start + 2] & 0x3f) << 12; 689 ret |= (str[start + 3] & 0x3f) << 6; 690 ret |= (str[start + 4] & 0x3f); 691 return ret; 692 } 693 if ((str[start] & 0xfe) == 0xfc && start + 6 <= str.size()) { 694 if (outNumBytes) 695 *outNumBytes = 6; 696 ret = (str[start] & 0x1) << 30; 697 ret |= (str[start + 1] & 0x3f) << 24; 698 ret |= (str[start + 2] & 0x3f) << 18; 699 ret |= (str[start + 3] & 0x3f) << 12; 700 ret |= (str[start + 4] & 0x3f) << 6; 701 ret |= (str[start + 5] & 0x3f); 702 return ret; 703 } 704 705 // invalid UTF8 706 if (outNumBytes) 707 *outNumBytes = 1; 708 return (uint32_t)str[start]; 709 } 710 SmoothStep(float v)711 float SmoothStep(float v) { return v * v * (3.f - 2.f * v); } 712 Mix(float a,float b,float frac)713 float Mix(float a, float b, float frac) { return a + (b - a) * frac; } Mix(const Vector2 & a,const Vector2 & b,float frac)714 Vector2 Mix(const Vector2 &a, const Vector2 &b, float frac) { return a + (b - a) * frac; } Mix(const Vector3 & a,const Vector3 & b,float frac)715 Vector3 Mix(const Vector3 &a, const Vector3 &b, float frac) { return a + (b - a) * frac; } 716 } 717