mathUtils.cpp
Engine/source/math/mathUtils.cpp
Classes:
Namespaces:
namespace
Miscellaneous math utility functions.
Detailed Description
Public Defines
MAX_TRIES() 20
MAX_TRIES() 20
1 2//----------------------------------------------------------------------------- 3// Copyright (c) 2012 GarageGames, LLC 4// 5// Permission is hereby granted, free of charge, to any person obtaining a copy 6// of this software and associated documentation files (the "Software"), to 7// deal in the Software without restriction, including without limitation the 8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9// sell copies of the Software, and to permit persons to whom the Software is 10// furnished to do so, subject to the following conditions: 11// 12// The above copyright notice and this permission notice shall be included in 13// all copies or substantial portions of the Software. 14// 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21// IN THE SOFTWARE. 22//----------------------------------------------------------------------------- 23 24#include "platform/platform.h" 25#include "math/util/frustum.h" 26#include "math/mathUtils.h" 27 28#include "math/mMath.h" 29#include "math/mRandom.h" 30#include "math/util/frustum.h" 31#include "platform/profiler.h" 32#include "core/tAlgorithm.h" 33 34namespace MathUtils 35{ 36 37MRandomLCG sgRandom(0xdeadbeef); ///< Our random number generator. 38 39//----------------------------------------------------------------------------- 40 41bool capsuleCapsuleOverlap(const Point3F & a1, const Point3F & b1, F32 rad1, const Point3F & a2, const Point3F & b2, F32 rad2) 42{ 43 F32 s,t; 44 Point3F c1,c2; 45 F32 dist = segmentSegmentNearest(a1,b1,a2,b2,s,t,c1,c2); 46 return dist <= (rad1+rad2)*(rad1+rad2); 47} 48 49//----------------------------------------------------------------------------- 50 51F32 segmentSegmentNearest(const Point3F & p1, const Point3F & q1, const Point3F & p2, const Point3F & q2, F32 & s, F32 & t, Point3F & c1, Point3F & c2) 52{ 53 Point3F d1 = q1-p1; 54 Point3F d2 = q2-p2; 55 Point3F r = p1-p2; 56 F32 a = mDot(d1,d1); 57 F32 e = mDot(d2,d2); 58 F32 f = mDot(d2,r); 59 60 const F32 EPSILON = 0.001f; 61 62 if (a <= EPSILON && e <= EPSILON) 63 { 64 s = t = 0.0f; 65 c1 = p1; 66 c2 = p2; 67 return mDot(c1-c2,c1-c2); 68 } 69 70 if (a <= EPSILON) 71 { 72 s = 0.0f; 73 t = mClampF(f/e,0.0f,1.0f); 74 } 75 else 76 { 77 F32 c = mDot(d1,r); 78 if (e <= EPSILON) 79 { 80 t = 0.0f; 81 s = mClampF(-c/a,0.0f,1.0f); 82 } 83 else 84 { 85 F32 b = mDot(d1,d2); 86 F32 denom = a*e-b*b; 87 if (denom != 0.0f) 88 s = mClampF((b*f-c*e)/denom,0.0f,1.0f); 89 else 90 s = 0.0f; 91 F32 tnom = b*s+f; 92 if (tnom < 0.0f) 93 { 94 t = 0.0f; 95 s = mClampF(-c/a,0.0f,1.0f); 96 } 97 else if (tnom>e) 98 { 99 t = 1.0f; 100 s = mClampF((b-c)/a,0.0f,1.0f); 101 } 102 else 103 t = tnom/e; 104 } 105 } 106 107 c1 = p1 + d1*s; 108 c2 = p2 + d2*t; 109 return mDot(c1-c2,c1-c2); 110} 111 112//----------------------------------------------------------------------------- 113 114bool capsuleSphereNearestOverlap(const Point3F & A0, const Point3F A1, F32 radA, const Point3F & B, F32 radB, F32 & t) 115{ 116 Point3F V = A1-A0; 117 Point3F A0B = A0-B; 118 F32 d1 = mDot(A0B,V); 119 F32 d2 = mDot(A0B,A0B); 120 F32 d3 = mDot(V,V); 121 F32 R2 = (radA+radB)*(radA+radB); 122 if (d2<R2) 123 { 124 // starting in collision state 125 t=0; 126 return true; 127 } 128 if (d3<0.01f) 129 // no movement, and don't start in collision state, so no collision 130 return false; 131 132 F32 b24ac = mSqrt(d1*d1-d2*d3+d3*R2); 133 F32 t1 = (-d1-b24ac)/d3; 134 if (t1>0 && t1<1.0f) 135 { 136 t=t1; 137 return true; 138 } 139 F32 t2 = (-d1+b24ac)/d3; 140 if (t2>0 && t2<1.0f) 141 { 142 t=t2; 143 return true; 144 } 145 if (t1<0 && t2>0) 146 { 147 t=0; 148 return true; 149 } 150 return false; 151} 152 153//----------------------------------------------------------------------------- 154 155void vectorRotateZAxis( Point3F &vector, F32 radians ) 156{ 157 F32 sin, cos; 158 mSinCos(radians, sin, cos); 159 F32 x = cos * vector.x - sin * vector.y; 160 F32 y = sin * vector.x + cos * vector.y; 161 vector.x = x; 162 vector.y = y; 163} 164 165void vectorRotateZAxis( F32 radians, Point3F *vectors, U32 count ) 166{ 167 F32 sin, cos; 168 mSinCos(radians, sin, cos); 169 170 F32 x, y; 171 const Point3F *end = vectors + count; 172 for ( ; vectors != end; vectors++ ) 173 { 174 x = cos * vectors->x - sin * vectors->y; 175 y = sin * vectors->x + cos * vectors->y; 176 vectors->x = x; 177 vectors->y = y; 178 } 179} 180 181//----------------------------------------------------------------------------- 182 183void getZBiasProjectionMatrix( F32 bias, const Frustum &frustum, MatrixF *outMat, bool rotate ) 184{ 185 Frustum temp(frustum); 186 temp.setNearDist(frustum.getNearDist() + bias); 187 temp.getProjectionMatrix(outMat, rotate); 188} 189 190//----------------------------------------------------------------------------- 191 192MatrixF createOrientFromDir( const Point3F &direction ) 193{ 194 Point3F j = direction; 195 Point3F k(0.0f, 0.0f, 1.0f); 196 Point3F i; 197 198 mCross( j, k, &i ); 199 200 if( i.magnitudeSafe() == 0.0f ) 201 { 202 i.set( 0.0f, -1.0f, 0.0f ); 203 } 204 205 i.normalizeSafe(); 206 mCross( i, j, &k ); 207 208 MatrixF mat( true ); 209 mat.setColumn( 0, i ); 210 mat.setColumn( 1, j ); 211 mat.setColumn( 2, k ); 212 213 return mat; 214} 215 216//----------------------------------------------------------------------------- 217 218void getMatrixFromUpVector( const VectorF &up, MatrixF *outMat ) 219{ 220 AssertFatal( up.isUnitLength(), "MathUtils::getMatrixFromUpVector() - Up vector was not normalized!" ); 221 AssertFatal( outMat, "MathUtils::getMatrixFromUpVector() - Got null output matrix!" ); 222 AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromUpVector() - Got uninitialized matrix!" ); 223 224 VectorF forward = mPerp( up ); 225 VectorF right = mCross( forward, up ); 226 right.normalize(); 227 forward = mCross( up, right ); 228 forward.normalize(); 229 230 outMat->setColumn( 0, right ); 231 outMat->setColumn( 1, forward ); 232 outMat->setColumn( 2, up ); 233} 234 235//----------------------------------------------------------------------------- 236 237void getMatrixFromForwardVector( const VectorF &forward, MatrixF *outMat ) 238{ 239 AssertFatal( forward.isUnitLength(), "MathUtils::getMatrixFromForwardVector() - Forward vector was not normalized!" ); 240 AssertFatal( outMat, "MathUtils::getMatrixFromForwardVector() - Got null output matrix!" ); 241 AssertFatal( outMat->isAffine(), "MathUtils::getMatrixFromForwardVector() - Got uninitialized matrix!" ); 242 243 VectorF up = mPerp( forward ); 244 VectorF right = mCross( forward, up ); 245 right.normalize(); 246 up = mCross( right, forward ); 247 up.normalize(); 248 249 outMat->setColumn( 0, right ); 250 outMat->setColumn( 1, forward ); 251 outMat->setColumn( 2, up ); 252} 253 254//----------------------------------------------------------------------------- 255 256Point3F randomDir( const Point3F &axis, F32 thetaAngleMin, F32 thetaAngleMax, 257 F32 phiAngleMin, F32 phiAngleMax ) 258{ 259 MatrixF orient = createOrientFromDir( axis ); 260 Point3F axisx; 261 orient.getColumn( 0, &axisx ); 262 263 F32 theta = (thetaAngleMax - thetaAngleMin) * sgRandom.randF() + thetaAngleMin; 264 F32 phi = (phiAngleMax - phiAngleMin) * sgRandom.randF() + phiAngleMin; 265 266 // Both phi and theta are in degs. Create axis angles out of them, and create the 267 // appropriate rotation matrix... 268 AngAxisF thetaRot(axisx, theta * (M_PI_F / 180.0f)); 269 AngAxisF phiRot(axis, phi * (M_PI_F / 180.0f)); 270 271 Point3F ejectionAxis = axis; 272 273 MatrixF temp(true); 274 thetaRot.setMatrix(&temp); 275 temp.mulP(ejectionAxis); 276 phiRot.setMatrix(&temp); 277 temp.mulP(ejectionAxis); 278 279 return ejectionAxis; 280} 281 282//----------------------------------------------------------------------------- 283 284Point3F randomPointInSphere( F32 radius ) 285{ 286 AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" ); 287 288 #define MAX_TRIES 20 289 290 Point3F out; 291 F32 radiusSq = radius * radius; 292 293 for ( S32 i = 0; i < MAX_TRIES; i++ ) 294 { 295 out.x = sgRandom.randF(-radius,radius); 296 out.y = sgRandom.randF(-radius,radius); 297 out.z = sgRandom.randF(-radius,radius); 298 299 if ( out.lenSquared() < radiusSq ) 300 return out; 301 } 302 303 AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." ); 304 return Point3F::Zero; 305} 306 307//----------------------------------------------------------------------------- 308 309Point2F randomPointInCircle( F32 radius ) 310{ 311 AssertFatal( radius > 0.0f, "MathUtils::randomPointInRadius - radius must be positive" ); 312 313 #define MAX_TRIES 20 314 315 Point2F out; 316 F32 radiusSq = radius * radius; 317 318 for ( S32 i = 0; i < MAX_TRIES; i++ ) 319 { 320 out.x = sgRandom.randF(-radius,radius); 321 out.y = sgRandom.randF(-radius,radius); 322 323 if ( out.lenSquared() < radiusSq ) 324 return out; 325 } 326 327 AssertFatal( false, "MathUtils::randomPointInRadius - something is wrong, should not fail this many times." ); 328 return Point2F::Zero; 329} 330 331//----------------------------------------------------------------------------- 332 333void getAnglesFromVector( const VectorF &vec, F32 &yawAng, F32 &pitchAng ) 334{ 335 yawAng = mAtan2( vec.x, vec.y ); 336 if( yawAng < 0.0f ) 337 yawAng += M_2PI_F; 338 339 if( mFabs(vec.x) > mFabs(vec.y) ) 340 pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.x) ); 341 else 342 pitchAng = mAtan2( mFabs(vec.z), mFabs(vec.y) ); 343 if( vec.z < 0.0f ) 344 pitchAng = -pitchAng; 345} 346 347//----------------------------------------------------------------------------- 348 349void getVectorFromAngles( VectorF &vec, F32 yawAng, F32 pitchAng ) 350{ 351 VectorF pnt( 0.0f, 1.0f, 0.0f ); 352 353 EulerF rot( -pitchAng, 0.0f, 0.0f ); 354 MatrixF mat( rot ); 355 356 rot.set( 0.0f, 0.0f, yawAng ); 357 MatrixF mat2( rot ); 358 359 mat.mulV( pnt ); 360 mat2.mulV( pnt ); 361 362 vec = pnt; 363} 364 365F32 getAngleBetweenVectors(VectorF vecA, VectorF vecB) 366{ 367 F32 dot = mDot(vecA, vecB); 368 F32 lenSq1 = vecA.lenSquared(); 369 F32 lenSq2 = vecB.lenSquared(); 370 F32 angle = mAcos(dot / mSqrt(lenSq1 * lenSq2)); 371 372 return angle; 373} 374 375//----------------------------------------------------------------------------- 376 377void transformBoundingBox(const Box3F &sbox, const MatrixF &mat, const Point3F scale, Box3F &dbox) 378{ 379 Point3F center; 380 381 // set transformed center... 382 sbox.getCenter(¢er); 383 center.convolve(scale); 384 mat.mulP(center); 385 dbox.minExtents = center; 386 dbox.maxExtents = center; 387 388 Point3F val; 389 for(U32 ix=0; ix<2; ix++) 390 { 391 if(ix & 0x1) 392 val.x = sbox.minExtents.x; 393 else 394 val.x = sbox.maxExtents.x; 395 396 for(U32 iy=0; iy<2; iy++) 397 { 398 if(iy & 0x1) 399 val.y = sbox.minExtents.y; 400 else 401 val.y = sbox.maxExtents.y; 402 403 for(U32 iz=0; iz<2; iz++) 404 { 405 if(iz & 0x1) 406 val.z = sbox.minExtents.z; 407 else 408 val.z = sbox.maxExtents.z; 409 410 Point3F v1, v2; 411 v1 = val; 412 v1.convolve(scale); 413 mat.mulP(v1, &v2); 414 dbox.minExtents.setMin(v2); 415 dbox.maxExtents.setMax(v2); 416 } 417 } 418 } 419} 420 421//----------------------------------------------------------------------------- 422 423bool mProjectWorldToScreen( const Point3F &in, 424 Point3F *out, 425 const RectI &view, 426 const MatrixF &world, 427 const MatrixF &projection ) 428{ 429 MatrixF worldProjection = projection; 430 worldProjection.mul(world); 431 432 return mProjectWorldToScreen( in, out, view, worldProjection ); 433} 434 435//----------------------------------------------------------------------------- 436 437bool mProjectWorldToScreen( const Point3F &in, 438 Point3F *out, 439 const RectI &view, 440 const MatrixF &worldProjection ) 441{ 442 Point4F temp(in.x,in.y,in.z,1.0f); 443 worldProjection.mul(temp); 444 445 // Perform the perspective division. For orthographic 446 // projections, temp.w will be 1. 447 448 temp.x /= temp.w; 449 temp.y /= temp.w; 450 temp.z /= temp.w; 451 452 // Take the normalized device coordinates (NDC) and transform them 453 // into device coordinates. 454 455 out->x = (temp.x + 1.0f) / 2.0f * view.extent.x + view.point.x; 456 out->y = (1.0f - temp.y) / 2.0f * view.extent.y + view.point.y; 457 out->z = temp.z; 458 459 if ( out->z < 0.0f || out->z > 1.0f || 460 out->x < (F32)view.point.x || out->x > (F32)view.point.x + (F32)view.extent.x || 461 out->y < (F32)view.point.y || out->y > (F32)view.point.y + (F32)view.extent.y ) 462 return false; 463 464 return true; 465} 466 467//----------------------------------------------------------------------------- 468 469void mProjectScreenToWorld( const Point3F &in, 470 Point3F *out, 471 const RectI &view, 472 const MatrixF &world, 473 const MatrixF &projection, 474 F32 zfar, 475 F32 znear ) 476{ 477 MatrixF invWorldProjection = projection; 478 invWorldProjection.mul(world); 479 invWorldProjection.inverse(); 480 481 Point3F vec; 482 vec.x = (in.x - view.point.x) * 2.0f / view.extent.x - 1.0f; 483 vec.y = -(in.y - view.point.y) * 2.0f / view.extent.y + 1.0f; 484 vec.z = (znear + in.z * (zfar - znear))/zfar; 485 486 invWorldProjection.mulV(vec); 487 vec *= 1.0f + in.z * zfar; 488 489 invWorldProjection.getColumn(3, out); 490 (*out) += vec; 491} 492 493//----------------------------------------------------------------------------- 494 495bool pointInPolygon( const Point2F *verts, U32 vertCount, const Point2F &testPt ) 496{ 497 U32 i, j, c = 0; 498 for ( i = 0, j = vertCount-1; i < vertCount; j = i++ ) 499 { 500 if ( ( ( verts[i].y > testPt.y ) != ( verts[j].y > testPt.y ) ) && 501 ( testPt.x < ( verts[j].x - verts[i].x ) * 502 ( testPt.y - verts[i].y ) / 503 ( verts[j].y - verts[i].y ) + verts[i].x ) ) 504 c = !c; 505 } 506 507 return c != 0; 508} 509 510//----------------------------------------------------------------------------- 511 512F32 mTriangleDistance( const Point3F &A, const Point3F &B, const Point3F &C, const Point3F &P, IntersectInfo* info ) 513{ 514 Point3F diff = A - P; 515 Point3F edge0 = B - A; 516 Point3F edge1 = C - A; 517 F32 a00 = edge0.lenSquared(); 518 F32 a01 = mDot( edge0, edge1 ); 519 F32 a11 = edge1.lenSquared(); 520 F32 b0 = mDot( diff, edge0 ); 521 F32 b1 = mDot( diff, edge1 ); 522 F32 c = diff.lenSquared(); 523 F32 det = mFabs(a00*a11-a01*a01); 524 F32 s = a01*b1-a11*b0; 525 F32 t = a01*b0-a00*b1; 526 F32 sqrDistance; 527 528 if (s + t <= det) 529 { 530 if (s < 0.0f) 531 { 532 if (t < 0.0f) // region 4 533 { 534 if (b0 < 0.0f) 535 { 536 t = 0.0f; 537 if (-b0 >= a00) 538 { 539 s = 1.0f; 540 sqrDistance = a00 + (2.0f)*b0 + c; 541 } 542 else 543 { 544 s = -b0/a00; 545 sqrDistance = b0*s + c; 546 } 547 } 548 else 549 { 550 s = 0.0f; 551 if (b1 >= 0.0f) 552 { 553 t = 0.0f; 554 sqrDistance = c; 555 } 556 else if (-b1 >= a11) 557 { 558 t = 1.0f; 559 sqrDistance = a11 + 2.0f*b1 + c; 560 } 561 else 562 { 563 t = -b1/a11; 564 sqrDistance = b1*t + c; 565 } 566 } 567 } 568 else // region 3 569 { 570 s = 0.0f; 571 if (b1 >= 0.0f) 572 { 573 t = 0.0f; 574 sqrDistance = c; 575 } 576 else if (-b1 >= a11) 577 { 578 t = 1.0f; 579 sqrDistance = a11 + 2.0f*b1 + c; 580 } 581 else 582 { 583 t = -b1/a11; 584 sqrDistance = b1*t + c; 585 } 586 } 587 } 588 else if (t < 0.0f) // region 5 589 { 590 t = 0.0f; 591 if (b0 >= 0.0f) 592 { 593 s = 0.0f; 594 sqrDistance = c; 595 } 596 else if (-b0 >= a00) 597 { 598 s = 1.0f; 599 sqrDistance = a00 + 2.0f*b0 + c; 600 } 601 else 602 { 603 s = -b0/a00; 604 sqrDistance = b0*s + c; 605 } 606 } 607 else // region 0 608 { 609 // minimum at interior point 610 F32 invDet = 1.0f / det; 611 s *= invDet; 612 t *= invDet; 613 sqrDistance = s * (a00*s + a01*t + 2.0f*b0) + 614 t * (a01*s + a11*t + 2.0f*b1) + c; 615 } 616 } 617 else 618 { 619 F32 tmp0, tmp1, numer, denom; 620 621 if (s < 0.0f) // region 2 622 { 623 tmp0 = a01 + b0; 624 tmp1 = a11 + b1; 625 if (tmp1 > tmp0) 626 { 627 numer = tmp1 - tmp0; 628 denom = a00 - 2.0f*a01 + a11; 629 if (numer >= denom) 630 { 631 s = 1.0f; 632 t = 0.0f; 633 sqrDistance = a00 + 2.0f*b0 + c; 634 } 635 else 636 { 637 s = numer/denom; 638 t = 1.0f - s; 639 sqrDistance = s * (a00*s + a01*t + 2.0f*b0) + 640 t * (a01*s + a11*t + 2.0f*b1) + c; 641 } 642 } 643 else 644 { 645 s = 0.0f; 646 if (tmp1 <= 0.0f) 647 { 648 t = 1.0f; 649 sqrDistance = a11 + 2.0f*b1 + c; 650 } 651 else if (b1 >= 0.0f) 652 { 653 t = 0.0f; 654 sqrDistance = c; 655 } 656 else 657 { 658 t = -b1/a11; 659 sqrDistance = b1*t + c; 660 } 661 } 662 } 663 else if (t < 0.0f) // region 6 664 { 665 tmp0 = a01 + b1; 666 tmp1 = a00 + b0; 667 if (tmp1 > tmp0) 668 { 669 numer = tmp1 - tmp0; 670 denom = a00 - 2.0f*a01 + a11; 671 if (numer >= denom) 672 { 673 t = 1.0f; 674 s = 0.0f; 675 sqrDistance = a11 + 2.0f*b1 + c; 676 } 677 else 678 { 679 t = numer/denom; 680 s = 1.0f - t; 681 sqrDistance = s * (a00*s + a01*t + 2.0f*b0) + 682 t * (a01*s + a11*t + 2.0f*b1) + c; 683 } 684 } 685 else 686 { 687 t = 0.0f; 688 if (tmp1 <= 0.0f) 689 { 690 s = 1.0f; 691 sqrDistance = a00 + 2.0f*b0 + c; 692 } 693 else if (b0 >= 0.0f) 694 { 695 s = 0.0f; 696 sqrDistance = c; 697 } 698 else 699 { 700 s = -b0/a00; 701 sqrDistance = b0*s + c; 702 } 703 } 704 } 705 else // region 1 706 { 707 numer = a11 + b1 - a01 - b0; 708 if (numer <= 0.0f) 709 { 710 s = 0.0f; 711 t = 1.0f; 712 sqrDistance = a11 + 2.0f*b1 + c; 713 } 714 else 715 { 716 denom = a00 - 2.0f*a01 + a11; 717 if (numer >= denom) 718 { 719 s = 1.0f; 720 t = 0.0f; 721 sqrDistance = a00 + 2.0f*b0 + c; 722 } 723 else 724 { 725 s = numer/denom; 726 t = 1.0f - s; 727 sqrDistance = s * (a00*s + a01*t + 2.0f*b0) + 728 t * (a01*s + a11*t + 2.0f*b1) + c; 729 } 730 } 731 } 732 } 733 734 // account for numerical round-off error 735 if (sqrDistance < 0.0f) 736 sqrDistance = 0.0f; 737 738 // This also calculates the barycentric coordinates and the closest point! 739 //m_kClosestPoint0 = P; 740 //m_kClosestPoint1 = A + s*edge0 + t*edge1; 741 //m_afTriangleBary[1] = s; 742 //m_afTriangleBary[2] = t; 743 //m_afTriangleBary[0] = (Real)1.0 - fS - fT; 744 if(info) 745 { 746 info->segment.p0 = P; 747 info->segment.p1 = A + s*edge0 + t*edge1; 748 info->bary.x = s; 749 info->bary.y = t; 750 info->bary.z = 1.0f - s - t; 751 } 752 753 return sqrDistance; 754} 755 756//----------------------------------------------------------------------------- 757 758Point3F mTriangleNormal( const Point3F &a, const Point3F &b, const Point3F &c ) 759{ 760 // Vector from b to a. 761 const F32 ax = a.x-b.x; 762 const F32 ay = a.y-b.y; 763 const F32 az = a.z-b.z; 764 // Vector from b to c. 765 const F32 cx = c.x-b.x; 766 const F32 cy = c.y-b.y; 767 const F32 cz = c.z-b.z; 768 769 Point3F n; 770 771 // This is an in-line cross product. 772 n.x = ay*cz - az*cy; 773 n.y = az*cx - ax*cz; 774 n.z = ax*cy - ay*cx; 775 m_point3F_normalize( (F32*)(&n) ); 776 777 return n; 778} 779 780//----------------------------------------------------------------------------- 781 782Point3F mClosestPointOnSegment( const Point3F &a, const Point3F &b, const Point3F &p ) 783{ 784 Point3F c = p - a; // Vector from a to Point 785 Point3F v = (b - a); 786 F32 d = v.len(); // Length of the line segment 787 v.normalize(); // Unit Vector from a to b 788 F32 t = mDot( v, c ); // Intersection point Distance from a 789 790 // Check to see if the point is on the line 791 // if not then return the endpoint 792 if(t < 0) return a; 793 if(t > d) return b; 794 795 // get the distance to move from point a 796 v *= t; 797 798 // move from point a to the nearest point on the segment 799 return a + v; 800} 801 802//----------------------------------------------------------------------------- 803 804void mShortestSegmentBetweenLines( const Line &line0, const Line &line1, LineSegment *outSegment ) 805{ 806 // compute intermediate parameters 807 Point3F w0 = line0.origin - line1.origin; 808 F32 a = mDot( line0.direction, line0.direction ); 809 F32 b = mDot( line0.direction, line1.direction ); 810 F32 c = mDot( line1.direction, line1.direction ); 811 F32 d = mDot( line0.direction, w0 ); 812 F32 e = mDot( line1.direction, w0 ); 813 814 F32 denom = a*c - b*b; 815 816 if ( denom > -0.001f && denom < 0.001f ) 817 { 818 outSegment->p0 = line0.origin; 819 outSegment->p1 = line1.origin + (e/c)*line1.direction; 820 } 821 else 822 { 823 outSegment->p0 = line0.origin + ((b*e - c*d)/denom)*line0.direction; 824 outSegment->p1 = line1.origin + ((a*e - b*d)/denom)*line1.direction; 825 } 826} 827 828//----------------------------------------------------------------------------- 829 830U32 greatestCommonDivisor( U32 u, U32 v ) 831{ 832 // http://en.wikipedia.org/wiki/Binary_GCD_algorithm 833 834 S32 shift; 835 836 /* GCD(0,x) := x */ 837 if (u == 0 || v == 0) 838 return u | v; 839 840 /* Left shift := lg K, where K is the greatest power of 2 841 dividing both u and v. */ 842 for (shift = 0; ((u | v) & 1) == 0; ++shift) { 843 u >>= 1; 844 v >>= 1; 845 } 846 847 while ((u & 1) == 0) 848 u >>= 1; 849 850 /* From here on, u is always odd. */ 851 do { 852 while ((v & 1) == 0) /* Loop X */ 853 v >>= 1; 854 855 /* Now u and v are both odd, so diff(u, v) is even. 856 Let u = min(u, v), v = diff(u, v)/2. */ 857 if (u < v) { 858 v -= u; 859 } else { 860 U32 diff = u - v; 861 u = v; 862 v = diff; 863 } 864 v >>= 1; 865 } while (v != 0); 866 867 return u << shift; 868} 869 870//----------------------------------------------------------------------------- 871 872bool mLineTriangleCollide( const Point3F &p1, const Point3F &p2, 873 const Point3F &t1, const Point3F &t2, const Point3F &t3, 874 Point3F *outUVW, F32 *outT ) 875{ 876 VectorF ab = t2 - t1; 877 VectorF ac = t3 - t1; 878 VectorF qp = p1 - p2; 879 880 // Compute triangle normal. Can be precalculated or cached if 881 // intersecting multiple segments against the same triangle 882 VectorF n = mCross( ab, ac ); 883 884 // Compute denominator d. If d <= 0, segment is parallel to or points 885 // away from triangle, so exit early 886 F32 d = mDot( qp, n ); 887 if ( d <= 0.0f ) 888 return false; 889 890 // Compute intersection t value of pq with plane of triangle. A ray 891 // intersects if 0 <= t. Segment intersects iff 0 <= t <= 1. Delay 892 // dividing by d until intersection has been found to pierce triangle 893 VectorF ap = p1 - t1; 894 F32 t = mDot( ap, n ); 895 if ( t < 0.0f ) 896 return false; 897 if ( t > d ) 898 return false; // For segment; exclude this code line for a ray test 899 900 // Compute barycentric coordinate components and test if within bounds 901 VectorF e = mCross( qp, ap ); 902 F32 v = mDot( ac, e ); 903 if ( v < 0.0f || v > d ) 904 return false; 905 F32 w = -mDot( ab, e ); 906 if ( w < 0.0f || v + w > d ) 907 return false; 908 909 // Segment/ray intersects triangle. Perform delayed division and 910 // compute the last barycentric coordinate component 911 const F32 ood = 1.0f / d; 912 913 if ( outT ) 914 *outT = t * ood; 915 916 if ( outUVW ) 917 { 918 v *= ood; 919 w *= ood; 920 outUVW->set( 1.0f - v - w, v, w ); 921 } 922 923 return true; 924} 925 926//----------------------------------------------------------------------------- 927 928bool mRayQuadCollide( const Quad &quad, 929 const Ray &ray, 930 Point2F *outUV, 931 F32 *outT ) 932{ 933 static const F32 eps = F32(10e-6); 934 935 // Rejects rays that are parallel to Q, and rays that intersect the plane of 936 // Q either on the left of the line V00V01 or on the right of the line V00V10. 937 938 // p01-----eXX-----p11 939 // ^ . ^ | 940 // | . | 941 // e03 e02 eXX 942 // | . | 943 // | . | 944 // p00-----e01---->p10 945 946 VectorF e01 = quad.p10 - quad.p00; 947 VectorF e03 = quad.p01 - quad.p00; 948 949 // If the ray is perfectly perpendicular to e03, which 950 // represents the entire planes tangent, then the 951 // result of this cross product (P) will equal e01 952 // If it is parallel it will result in a vector opposite e01. 953 954 // If the ray is heading DOWN the cross product will point to the RIGHT 955 // If the ray is heading UP the cross product will point to the LEFT 956 // We do not reject based on this though... 957 // 958 // In either case cross product will be more parallel to e01 the more 959 // perpendicular the ray is to e03, and it will be more perpendicular to 960 // e01 the more parallel it is to e03. 961 VectorF P = mCross(ray.direction, e03); 962 963 // det can be seen as 'the amount of vector e01 in the direction P' 964 F32 det = mDot(e01, P); 965 966 // Take a Abs of the dot because we do not care if the ray is heading up or down, 967 // but if it is perfectly parallel to the quad we want to reject it. 968 if ( mFabs(det) < eps ) 969 return false; 970 971 F32 inv_det = 1.0f / det; 972 973 VectorF T = ray.origin - quad.p00; 974 975 // alpha can be seen as 'the amount of vector T in the direction P' 976 // T is a vector up from the quads corner point 00 to the ray's origin. 977 // P is the cross product of the ray and e01, which should be "roughly" 978 // parallel with e03 but might be of either positive or negative magnitude. 979 F32 alpha = mDot(T, P) * inv_det; 980 if ( alpha < 0.0f ) 981 return false; 982 983 // if (alpha > real(1.0)) return false; // Uncomment if VR is used. 984 985 // The cross product of T and e01 should be roughly parallel to e03 986 // and of either positive or negative magnitude. 987 VectorF Q = mCross(T, e01); 988 F32 beta = mDot(ray.direction, Q) * inv_det; 989 if ( beta < 0.0f ) 990 return false; 991 992 // if (beta > real(1.0)) return false; // Uncomment if VR is used. 993 994 if ( alpha + beta > 1.0f ) 995 //if ( false ) 996 { 997 // Rejects rays that intersect the plane of Q either on the 998 // left of the line V11V10 or on the right of the line V11V01. 999 1000 VectorF e23 = quad.p01 - quad.p11; 1001 VectorF e21 = quad.p10 - quad.p11; 1002 VectorF P_prime = mCross(ray.direction, e21); 1003 F32 det_prime = mDot(e23, P_prime); 1004 if ( mFabs(det_prime) < eps) 1005 return false; 1006 F32 inv_det_prime = 1.0f / det_prime; 1007 VectorF T_prime = ray.origin - quad.p11; 1008 F32 alpha_prime = mDot(T_prime, P_prime) * inv_det_prime; 1009 if (alpha_prime < 0.0f) 1010 return false; 1011 VectorF Q_prime = mCross(T_prime, e23); 1012 F32 beta_prime = mDot(ray.direction, Q_prime) * inv_det_prime; 1013 if (beta_prime < 0.0f) 1014 return false; 1015 } 1016 1017 // Compute the ray parameter of the intersection point, and 1018 // reject the ray if it does not hit Q. 1019 1020 F32 t = mDot(e03, Q) * inv_det; 1021 if ( t < 0.0f ) 1022 return false; 1023 1024 1025 // Compute the barycentric coordinates of the fourth vertex. 1026 // These do not depend on the ray, and can be precomputed 1027 // and stored with the quadrilateral. 1028 1029 F32 alpha_11, beta_11; 1030 VectorF e02 = quad.p11 - quad.p00; 1031 VectorF n = mCross(e01, e03); 1032 1033 if ( mFabs(n.x) >= mFabs(n.y) && 1034 mFabs(n.x) >= mFabs(n.z) ) 1035 { 1036 alpha_11 = ( e02.y * e03.z - e02.z * e03.y ) / n.x; 1037 beta_11 = ( e01.y * e02.z - e01.z * e02.y ) / n.x; 1038 } 1039 else if ( mFabs(n.y) >= mFabs(n.x) && 1040 mFabs(n.y) >= mFabs(n.z) ) 1041 { 1042 alpha_11 = ((e02.z * e03.x) - (e02.x * e03.z)) / n.y; 1043 beta_11 = ((e01.z * e02.x) - (e01.x * e02.z)) / n.y; 1044 } 1045 else 1046 { 1047 alpha_11 = ((e02.x * e03.y) - (e02.y * e03.x)) / n.z; 1048 beta_11 = ((e01.x * e02.y) - (e01.y * e02.x)) / n.z; 1049 } 1050 1051 // Compute the bilinear coordinates of the intersection point. 1052 1053 F32 u,v; 1054 1055 if ( mFabs(alpha_11 - 1.0f) < eps) 1056 { 1057 // Q is a trapezium. 1058 u = alpha; 1059 if ( mFabs(beta_11 - 1.0f) < eps) 1060 v = beta; // Q is a parallelogram. 1061 else 1062 v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); // Q is a trapezium. 1063 } 1064 else if ( mFabs(beta_11 - 1.0f) < eps) 1065 { 1066 // Q is a trapezium. 1067 v = beta; 1068 u = alpha / ((v * (alpha_11 - 1.0f)) + 1.0f); 1069 } 1070 else 1071 { 1072 F32 A = 1.0f - beta_11; 1073 F32 B = (alpha * (beta_11 - 1.0f)) 1074 - (beta * (alpha_11 - 1.0f)) - 1.0f; 1075 F32 C = alpha; 1076 F32 D = (B * B) - (4.0f * A * C); 1077 F32 Q = -0.5f * (B + (B < 0.0f ? -1.0f : 1.0f) ) * mSqrt(D); 1078 u = Q / A; 1079 if ((u < 0.0f) || (u > 1.0f)) u = C / Q; 1080 v = beta / ((u * (beta_11 - 1.0f)) + 1.0f); 1081 } 1082 1083 if ( outUV ) 1084 outUV->set( u, v ); 1085 if ( outT ) 1086 *outT = t; 1087 1088 return true; 1089} 1090 1091//----------------------------------------------------------------------------- 1092 1093// Used by sortQuadWindingOrder. 1094struct QuadSortPoint 1095{ 1096 U32 id; 1097 F32 theta; 1098}; 1099 1100// Used by sortQuadWindingOrder. 1101S32 QSORT_CALLBACK cmpAngleAscending( const void *a, const void *b ) 1102{ 1103 const QuadSortPoint *p0 = (const QuadSortPoint*)a; 1104 const QuadSortPoint *p1 = (const QuadSortPoint*)b; 1105 1106 F32 diff = p1->theta - p0->theta; 1107 1108 if ( diff > 0.0f ) 1109 return -1; 1110 else if ( diff < 0.0f ) 1111 return 1; 1112 else 1113 return 0; 1114} 1115 1116// Used by sortQuadWindingOrder. 1117S32 QSORT_CALLBACK cmpAngleDescending( const void *a, const void *b ) 1118{ 1119 const QuadSortPoint *p0 = (const QuadSortPoint*)a; 1120 const QuadSortPoint *p1 = (const QuadSortPoint*)b; 1121 1122 F32 diff = p1->theta - p0->theta; 1123 1124 if ( diff > 0.0f ) 1125 return 1; 1126 else if ( diff < 0.0f ) 1127 return -1; 1128 else 1129 return 0; 1130} 1131 1132void sortQuadWindingOrder( const MatrixF &quadMat, bool clockwise, const Point3F *verts, U32 *vertMap, U32 count ) 1133{ 1134 PROFILE_SCOPE( MathUtils_sortQuadWindingOrder ); 1135 1136 if ( count == 0 ) 1137 return; 1138 1139 Point3F *quadPoints = new Point3F[count]; 1140 1141 for ( S32 i = 0; i < count; i++ ) 1142 { 1143 quadMat.mulP( verts[i], &quadPoints[i] ); 1144 quadPoints[i].normalizeSafe(); 1145 } 1146 1147 sortQuadWindingOrder( clockwise, quadPoints, vertMap, count ); 1148 1149 delete [] quadPoints; 1150} 1151 1152void sortQuadWindingOrder( bool clockwise, const Point3F *verts, U32 *vertMap, U32 count ) 1153{ 1154 QuadSortPoint *sortPoints = new QuadSortPoint[count]; 1155 1156 for ( S32 i = 0; i < count; i++ ) 1157 { 1158 QuadSortPoint &sortPnt = sortPoints[i]; 1159 const Point3F &vec = verts[i]; 1160 1161 sortPnt.id = i; 1162 1163 F32 theta = mAtan2( vec.y, vec.x ); 1164 1165 if ( vec.y < 0.0f ) 1166 theta = M_2PI_F + theta; 1167 1168 sortPnt.theta = theta; 1169 } 1170 1171 dQsort( sortPoints, count, sizeof( QuadSortPoint ), clockwise ? cmpAngleDescending : cmpAngleAscending ); 1172 1173 for ( S32 i = 0; i < count; i++ ) 1174 vertMap[i] = sortPoints[i].id; 1175 1176 delete [] sortPoints; 1177} 1178 1179//----------------------------------------------------------------------------- 1180 1181void buildMatrix( const VectorF *rvec, const VectorF *fvec, const VectorF *uvec, const VectorF *pos, MatrixF *outMat ) 1182{ 1183 /// Work in Progress 1184 1185 /* 1186 AssertFatal( !rvec || rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" ); 1187 AssertFatal( !fvec || fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" ); 1188 AssertFatal( !uvec || uvec->isUnitLength(), "MathUtils::buildMatrix() - Up vector was not normalized!" ); 1189 1190 // Note this relationship: 1191 // 1192 // Column0 Column1 Column2 1193 // Axis X Axis Y Axis Z 1194 // Rvec Fvec Uvec 1195 // 1196 1197 enum 1198 { 1199 RVEC = 1, 1200 FVEC = 1 << 1, 1201 UVEC = 1 << 2, 1202 ALL = RVEC | FVEC | UVEC 1203 }; 1204 1205 U8 mask = 0; 1206 U8 count = 0; 1207 U8 axis0, axis1; 1208 1209 if ( rvec ) 1210 { 1211 mask |= RVEC; 1212 axis0 == 0; 1213 count++; 1214 } 1215 if ( fvec ) 1216 { 1217 mask |= FVEC; 1218 if ( count == 0 ) 1219 axis0 = 1; 1220 else 1221 axis1 = 1; 1222 count++; 1223 } 1224 if ( uvec ) 1225 { 1226 mask |= UVEC; 1227 count++; 1228 } 1229 1230 U8 bR = 1; 1231 U8 bF = 1 << 1; 1232 U8 bU = 1 << 2; 1233 U8 bRF = bR | bF; 1234 U8 bRU = bR | bU; 1235 U8 bFU = bF | bU; 1236 U8 bRFU = bR | bF | bU; 1237 1238 1239 1240 // Cross product map. 1241 U8 cpdMap[3][2] = 1242 { 1243 { 1, 2 }, 1244 { 2, 0 }, 1245 { 0, 1 }, 1246 } 1247 1248 if ( count == 1 ) 1249 { 1250 if ( mask == bR ) 1251 { 1252 1253 } 1254 else if ( mask == bF ) 1255 { 1256 1257 } 1258 else if ( mask == bU ) 1259 { 1260 1261 } 1262 } 1263 else if ( count == 2 ) 1264 { 1265 if ( mask == bRF ) 1266 { 1267 1268 } 1269 else if ( mask == bRU ) 1270 { 1271 1272 } 1273 else if ( mask == bFU ) 1274 { 1275 1276 } 1277 } 1278 else // bRFU 1279 { 1280 1281 } 1282 1283 if ( rvec ) 1284 { 1285 outMat->setColumn( 0, *rvec ); 1286 1287 if ( fvec ) 1288 { 1289 outMat->setColumn( 1, *fvec ); 1290 1291 if ( uvec ) 1292 outMat->setColumn( 2, *uvec ); 1293 else 1294 { 1295 // Set uvec from rvec/fvec 1296 tmp = mCross( rvec, fvec ); 1297 tmp.normalizeSafe(); 1298 outMat->setColumn( 2, tmp ); 1299 } 1300 } 1301 else if ( uvec ) 1302 { 1303 // Set fvec from uvec/rvec 1304 tmp = mCross( uvec, rvec ); 1305 tmp.normalizeSafe(); 1306 outMat->setColumn( 1, tmp ); 1307 } 1308 else 1309 { 1310 // Set fvec and uvec from rvec 1311 Point3F tempFvec = mPerp( rvec ); 1312 Point3F tempUvec = mCross( ) 1313 1314 } 1315 } 1316 AssertFatal( rvec->isUnitLength(), "MathUtils::buildMatrix() - Right vector was not normalized!" ); 1317 AssertFatal( fvec->isUnitLength(), "MathUtils::buildMatrix() - Forward vector was not normalized!" ); 1318 AssertFatal( uvec->isUnitLength(), "MathUtils::buildMatrix() - UpVector vector was not normalized!" ); 1319 AssertFatal( outMat, "MathUtils::buildMatrix() - Got null output matrix!" ); 1320 AssertFatal( outMat->isAffine(), "MathUtils::buildMatrix() - Got uninitialized matrix!" ); 1321 */ 1322} 1323 1324//----------------------------------------------------------------------------- 1325 1326bool reduceFrustum( const Frustum& frustum, const RectI& viewport, const RectF& area, Frustum& outFrustum ) 1327{ 1328 // Just to be safe, clamp the area to the viewport. 1329 1330 Point2F clampedMin; 1331 Point2F clampedMax; 1332 1333 clampedMin.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x ); 1334 clampedMin.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y ); 1335 1336 clampedMax.x = mClampF( area.extent.x, ( F32 ) viewport.point.x, ( F32 ) viewport.point.x + viewport.extent.x ); 1337 clampedMax.y = mClampF( area.extent.y, ( F32 ) viewport.point.y, ( F32 ) viewport.point.y + viewport.extent.y ); 1338 1339 // If we have ended up without a visible region on the screen, 1340 // terminate now. 1341 1342 if( mFloor( clampedMin.x ) == mFloor( clampedMax.x ) || 1343 mFloor( clampedMin.y ) == mFloor( clampedMax.y ) ) 1344 return false; 1345 1346 // Get the extents of the frustum. 1347 1348 const F32 frustumXExtent = mFabs( frustum.getNearRight() - frustum.getNearLeft() ); 1349 const F32 frustumYExtent = mFabs( frustum.getNearTop() - frustum.getNearBottom() ); 1350 1351 // Now, normalize the screen-space pixel coordinates to lie within the screen-centered 1352 // -1 to 1 coordinate space that is used for the frustum planes. 1353 1354 Point2F normalizedMin; 1355 Point2F normalizedMax; 1356 1357 normalizedMin.x = ( ( clampedMin.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f ); 1358 normalizedMin.y = ( ( clampedMin.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f ); 1359 normalizedMax.x = ( ( clampedMax.x / viewport.extent.x ) * frustumXExtent ) - ( frustumXExtent / 2.f ); 1360 normalizedMax.y = ( ( clampedMax.y / viewport.extent.y ) * frustumYExtent ) - ( frustumYExtent / 2.f ); 1361 1362 // Make sure the generated frustum metrics are somewhat sane. 1363 1364 if( normalizedMax.x - normalizedMin.x < 0.001f || 1365 normalizedMax.y - normalizedMin.y < 0.001f ) 1366 return false; 1367 1368 // Finally, create the new frustum using the original's frustum 1369 // information except its left/right/top/bottom planes. 1370 // 1371 // Note that screen-space coordinates go upside down on Y whereas 1372 // camera-space frustum coordinates go downside up on Y which is 1373 // why we are inverting Y here. 1374 1375 outFrustum.set( 1376 frustum.isOrtho(), 1377 normalizedMin.x, 1378 normalizedMax.x, 1379 - normalizedMin.y, 1380 - normalizedMax.y, 1381 frustum.getNearDist(), 1382 frustum.getFarDist(), 1383 frustum.getTransform() 1384 ); 1385 1386 return true; 1387} 1388 1389//----------------------------------------------------------------------------- 1390 1391void makeFrustum( F32 *outLeft, 1392 F32 *outRight, 1393 F32 *outTop, 1394 F32 *outBottom, 1395 F32 fovYInRadians, 1396 F32 aspectRatio, 1397 F32 nearPlane ) 1398{ 1399 F32 top = nearPlane * mTan( fovYInRadians / 2.0 ); 1400 if ( outTop ) *outTop = top; 1401 if ( outBottom ) *outBottom = -top; 1402 1403 F32 left = top * aspectRatio; 1404 if ( outLeft ) *outLeft = -left; 1405 if ( outRight ) *outRight = left; 1406} 1407 1408//----------------------------------------------------------------------------- 1409 1410void makeProjection( MatrixF *outMatrix, 1411 F32 fovYInRadians, 1412 F32 aspectRatio, 1413 F32 nearPlane, 1414 F32 farPlane, 1415 bool gfxRotate ) 1416{ 1417 F32 left, right, top, bottom; 1418 makeFrustum( &left, &right, &top, &bottom, fovYInRadians, aspectRatio, nearPlane ); 1419 makeProjection( outMatrix, left, right, top, bottom, nearPlane, farPlane, gfxRotate ); 1420} 1421 1422//----------------------------------------------------------------------------- 1423 1424void makeFovPortFrustum( 1425 Frustum *outFrustum, 1426 bool isOrtho, 1427 F32 nearDist, 1428 F32 farDist, 1429 const FovPort &inPort, 1430 const MatrixF &transform) 1431{ 1432 F32 leftSize = nearDist * inPort.leftTan; 1433 F32 rightSize = nearDist * inPort.rightTan; 1434 F32 upSize = nearDist * inPort.upTan; 1435 F32 downSize = nearDist * inPort.downTan; 1436 1437 F32 left = -leftSize; 1438 F32 right = rightSize; 1439 F32 top = upSize; 1440 F32 bottom = -downSize; 1441 1442 outFrustum->set(isOrtho, left, right, top, bottom, nearDist, farDist, transform); 1443} 1444 1445//----------------------------------------------------------------------------- 1446 1447/// This is the special rotation matrix applied to 1448/// projection matricies for GFX. 1449/// 1450/// It is a wart of the OGL to DX change over. 1451/// 1452static const MatrixF sGFXProjRotMatrix( EulerF( (M_PI_F / 2.0f), 0.0f, 0.0f ) ); 1453 1454void makeProjection( MatrixF *outMatrix, 1455 F32 left, 1456 F32 right, 1457 F32 top, 1458 F32 bottom, 1459 F32 nearPlane, 1460 F32 farPlane, 1461 bool gfxRotate ) 1462{ 1463 Point4F row; 1464 row.x = 2.0*nearPlane / (right-left); 1465 row.y = 0.0; 1466 row.z = 0.0; 1467 row.w = 0.0; 1468 outMatrix->setRow( 0, row ); 1469 1470 row.x = 0.0; 1471 row.y = 2.0 * nearPlane / (top-bottom); 1472 row.z = 0.0; 1473 row.w = 0.0; 1474 outMatrix->setRow( 1, row ); 1475 1476 row.x = (left+right) / (right-left); 1477 row.y = (top+bottom) / (top-bottom); 1478 row.z = farPlane / (nearPlane - farPlane); 1479 row.w = -1.0; 1480 outMatrix->setRow( 2, row ); 1481 1482 row.x = 0.0; 1483 row.y = 0.0; 1484 row.z = nearPlane * farPlane / (nearPlane - farPlane); 1485 row.w = 0.0; 1486 outMatrix->setRow( 3, row ); 1487 1488 outMatrix->transpose(); 1489 1490 if ( gfxRotate ) 1491 outMatrix->mul( sGFXProjRotMatrix ); 1492} 1493 1494//----------------------------------------------------------------------------- 1495 1496void makeOrthoProjection( MatrixF *outMatrix, 1497 F32 left, 1498 F32 right, 1499 F32 top, 1500 F32 bottom, 1501 F32 nearPlane, 1502 F32 farPlane, 1503 bool gfxRotate ) 1504{ 1505 Point4F row; 1506 row.x = 2.0f / (right - left); 1507 row.y = 0.0f; 1508 row.z = 0.0f; 1509 row.w = 0.0f; 1510 outMatrix->setRow( 0, row ); 1511 1512 row.x = 0.0f; 1513 row.y = 2.0f / (top - bottom); 1514 row.z = 0.0f; 1515 row.w = 0.0f; 1516 outMatrix->setRow( 1, row ); 1517 1518 row.x = 0.0f; 1519 row.y = 0.0f; 1520 row.w = 0.0f; 1521 1522 //Unlike D3D, which has a 0-1 range, OpenGL uses a -1-1 range. 1523 //However, epoxy internally handles the swap, so the math here is the same for both APIs 1524 row.z = 1.0f / (nearPlane - farPlane); 1525 1526 outMatrix->setRow( 2, row ); 1527 1528 row.x = (left + right) / (left - right); 1529 row.y = (top + bottom) / (bottom - top); 1530 row.z = nearPlane / (nearPlane - farPlane); 1531 row.w = 1.0f; 1532 outMatrix->setRow( 3, row ); 1533 1534 outMatrix->transpose(); 1535 1536 if ( gfxRotate ) 1537 outMatrix->mul( sGFXProjRotMatrix ); 1538} 1539 1540//----------------------------------------------------------------------------- 1541 1542bool edgeFaceIntersect( const Point3F &edgeA, const Point3F &edgeB, 1543 const Point3F &faceA, const Point3F &faceB, const Point3F &faceC, const Point3F &faceD, Point3F *intersection ) 1544{ 1545 VectorF edgeAB = edgeB - edgeA; 1546 VectorF edgeAFaceA = faceA - edgeA; 1547 VectorF edgeAFaceB = faceB - edgeA; 1548 VectorF edgeAFaceC = faceC - edgeA; 1549 1550 VectorF m = mCross( edgeAFaceC, edgeAB ); 1551 F32 v = mDot( edgeAFaceA, m ); 1552 if ( v >= 0.0f ) 1553 { 1554 F32 u = -mDot( edgeAFaceB, m ); 1555 if ( u < 0.0f ) 1556 return false; 1557 1558 VectorF tmp = mCross( edgeAFaceB, edgeAB ); 1559 F32 w = mDot( edgeAFaceA, tmp ); 1560 if ( w < 0.0f ) 1561 return false; 1562 1563 F32 denom = 1.0f / (u + v + w ); 1564 u *= denom; 1565 v *= denom; 1566 w *= denom; 1567 1568 (*intersection) = u * faceA + v * faceB + w * faceC; 1569 } 1570 else 1571 { 1572 VectorF edgeAFaceD = faceD - edgeA; 1573 F32 u = mDot( edgeAFaceD, m ); 1574 if ( u < 0.0f ) 1575 return false; 1576 1577 VectorF tmp = mCross( edgeAFaceA, edgeAB ); 1578 F32 w = mDot( edgeAFaceD, tmp ); 1579 if ( w < 0.0f ) 1580 return false; 1581 1582 v = -v; 1583 1584 F32 denom = 1.0f / ( u + v + w ); 1585 u *= denom; 1586 v *= denom; 1587 w *= denom; 1588 1589 (*intersection) = u * faceA + v * faceD + w * faceC; 1590 } 1591 1592 return true; 1593} 1594 1595//----------------------------------------------------------------------------- 1596 1597bool isPlanarPolygon( const Point3F* vertices, U32 numVertices ) 1598{ 1599 AssertFatal( vertices != NULL, "MathUtils::isPlanarPolygon - Received NULL pointer" ); 1600 AssertFatal( numVertices >= 3, "MathUtils::isPlanarPolygon - Must have at least three vertices" ); 1601 1602 // Triangles are always planar. Letting smaller numVertices 1603 // slip through provides robustness for errors in release builds. 1604 1605 if( numVertices <= 3 ) 1606 return true; 1607 1608 // Compute the normal of the first triangle in the polygon. 1609 1610 Point3F triangle1Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ 2 ] ); 1611 1612 // Now go through all the remaining vertices and build triangles 1613 // with the first two vertices. Then the normals of all these triangles 1614 // must be the same (minus some variance due to floating-point inaccuracies) 1615 // as the normal of the first triangle. 1616 1617 for( U32 i = 3; i < numVertices; ++ i ) 1618 { 1619 Point3F triangle2Normal = mTriangleNormal( vertices[ 0 ], vertices[ 1 ], vertices[ i ] ); 1620 if( !triangle1Normal.equal( triangle2Normal ) ) 1621 return false; 1622 } 1623 1624 return true; 1625} 1626 1627//----------------------------------------------------------------------------- 1628 1629bool isConvexPolygon( const Point3F* vertices, U32 numVertices ) 1630{ 1631 AssertFatal( vertices != NULL, "MathUtils::isConvexPolygon - Received NULL pointer" ); 1632 AssertFatal( numVertices >= 3, "MathUtils::isConvexPolygon - Must have at least three vertices" ); 1633 1634 // Triangles are always convex. Letting smaller numVertices 1635 // slip through provides robustness for errors in release builds. 1636 1637 if( numVertices <= 3 ) 1638 return true; 1639 1640 U32 numPositive = 0; 1641 U32 numNegative = 0; 1642 1643 for( U32 i = 0; i < numVertices; ++ i ) 1644 { 1645 const Point3F& a = vertices[ i ]; 1646 const Point3F& b = vertices[ ( i + 1 ) % numVertices ]; 1647 const Point3F& c = vertices[ ( i + 2 ) % numVertices ]; 1648 1649 const F32 crossProductLength = mCross( b - a, c - b ).len(); 1650 1651 if( crossProductLength < 0.f ) 1652 numNegative ++; 1653 else if( crossProductLength > 0.f ) 1654 numPositive ++; 1655 1656 if( numNegative && numPositive ) 1657 return false; 1658 } 1659 1660 return true; 1661} 1662 1663//----------------------------------------------------------------------------- 1664 1665bool clipFrustumByPolygon( const Point3F* points, U32 numPoints, const RectI& viewport, const MatrixF& world, 1666 const MatrixF& projection, const Frustum& inFrustum, const Frustum& rootFrustum, Frustum& outFrustum ) 1667{ 1668 enum 1669 { 1670 MAX_RESULT_VERTICES = 64, 1671 MAX_INPUT_VERTICES = MAX_RESULT_VERTICES - Frustum::PlaneCount // Clipping against each plane may add a vertex. 1672 }; 1673 1674 AssertFatal( numPoints <= MAX_INPUT_VERTICES, "MathUtils::clipFrustumByPolygon - Too many vertices!" ); 1675 if( numPoints > MAX_INPUT_VERTICES ) 1676 return false; 1677 1678 // First, we need to clip the polygon against inFrustum. 1679 // 1680 // Use two buffers here in interchanging roles as sources and targets 1681 // in clipping against the frustum planes. 1682 1683 Point3F polygonBuffer1[ MAX_RESULT_VERTICES ]; 1684 Point3F polygonBuffer2[ MAX_RESULT_VERTICES ]; 1685 1686 Point3F* tempPolygon = polygonBuffer1; 1687 Point3F* clippedPolygon = polygonBuffer2; 1688 1689 dMemcpy( clippedPolygon, points, numPoints * sizeof( points[ 0 ] ) ); 1690 1691 U32 numClippedPolygonVertices = numPoints; 1692 U32 numTempPolygonVertices = 0; 1693 1694 for( U32 nplane = 0; nplane < Frustum::PlaneCount; ++ nplane ) 1695 { 1696 // Make the output of the last iteration the 1697 // input of this iteration. 1698 1699 swap( tempPolygon, clippedPolygon ); 1700 numTempPolygonVertices = numClippedPolygonVertices; 1701 1702 // Clip our current remainder of the original polygon 1703 // against the current plane. 1704 1705 const PlaneF& plane = inFrustum.getPlanes()[ nplane ]; 1706 numClippedPolygonVertices = plane.clipPolygon( tempPolygon, numTempPolygonVertices, clippedPolygon ); 1707 1708 // If the polygon was completely on the backside of the plane, 1709 // then polygon is outside the frustum. In this case, return false 1710 // to indicate we haven't clipped anything. 1711 1712 if( !numClippedPolygonVertices ) 1713 return false; 1714 } 1715 1716 // Project the clipped polygon into screen space. 1717 1718 MatrixF worldProjection = projection; 1719 worldProjection.mul( world ); // Premultiply world*projection so we don't have to do this over and over for each point. 1720 1721 Point3F projectedPolygon[ 10 ]; 1722 for( U32 i = 0; i < numClippedPolygonVertices; ++ i ) 1723 mProjectWorldToScreen( 1724 clippedPolygon[ i ], 1725 &projectedPolygon[ i ], 1726 viewport, 1727 worldProjection 1728 ); 1729 1730 // Put an axis-aligned rectangle around our polygon. 1731 1732 Point2F minPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y ); 1733 Point2F maxPoint( projectedPolygon[ 0 ].x, projectedPolygon[ 0 ].y ); 1734 1735 for( U32 i = 1; i < numClippedPolygonVertices; ++ i ) 1736 { 1737 minPoint.setMin( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) ); 1738 maxPoint.setMax( Point2F( projectedPolygon[ i ].x, projectedPolygon[ i ].y ) ); 1739 } 1740 1741 RectF area( minPoint, maxPoint - minPoint ); 1742 1743 // Finally, reduce the input frustum to the given area. Note that we 1744 // use rootFrustum here instead of inFrustum as the latter does not necessarily 1745 // represent the full viewport we are using here which thus would skew the mapping. 1746 1747 return reduceFrustum( rootFrustum, viewport, area, outFrustum ); 1748} 1749 1750//----------------------------------------------------------------------------- 1751 1752U32 extrudePolygonEdges( const Point3F* vertices, U32 numVertices, const Point3F& direction, PlaneF* outPlanes ) 1753{ 1754 U32 numPlanes = 0; 1755 U32 lastVertex = numVertices - 1; 1756 bool invert = false; 1757 1758 for( U32 i = 0; i < numVertices; lastVertex = i, ++ i ) 1759 { 1760 const Point3F& v1 = vertices[ i ]; 1761 const Point3F& v2 = vertices[ lastVertex ]; 1762 1763 // Skip the edge if it's length is really short. 1764 1765 const Point3F edgeVector = v2 - v1; 1766 if( edgeVector.len() < 0.05 ) 1767 continue; 1768 1769 // Compute the plane normal. The direction and the edge vector 1770 // basically define the orientation of the plane so their cross 1771 // product is the plane normal. 1772 1773 Point3F normal; 1774 if( !invert ) 1775 normal = mCross( edgeVector, direction ); 1776 else 1777 normal = mCross( direction, edgeVector ); 1778 1779 // Create a plane for the edge. 1780 1781 outPlanes[ numPlanes ] = PlaneF( v1, normal ); 1782 numPlanes ++; 1783 1784 // If this is the first plane that we have created, find out whether 1785 // the vertex ordering is giving us the plane orientations that we want 1786 // (facing inside). If not, invert vertex order from now on. 1787 1788 if( i == 0 ) 1789 { 1790 const PlaneF& plane = outPlanes[ numPlanes - 1 ]; 1791 for( U32 n = i + 1; n < numVertices; ++ n ) 1792 { 1793 const PlaneF::Side side = plane.whichSide( vertices[ n ] ); 1794 if( side == PlaneF::On ) 1795 continue; 1796 1797 if( side != PlaneF::Front ) 1798 invert = true; 1799 break; 1800 } 1801 } 1802 } 1803 1804 return numPlanes; 1805} 1806 1807//----------------------------------------------------------------------------- 1808 1809U32 extrudePolygonEdgesFromPoint( const Point3F* vertices, U32 numVertices, const Point3F& fromPoint, PlaneF* outPlanes ) 1810{ 1811 U32 numPlanes = 0; 1812 U32 lastVertex = numVertices - 1; 1813 bool invert = false; 1814 1815 for( U32 i = 0; i < numVertices; lastVertex = i, ++ i ) 1816 { 1817 const Point3F& v1 = vertices[ i ]; 1818 const Point3F& v2 = vertices[ lastVertex ]; 1819 1820 // Skip the edge if it's length is really short. 1821 1822 const Point3F edgeVector = v2 - v1; 1823 if( edgeVector.len() < 0.05 ) 1824 continue; 1825 1826 // Create a plane for the edge. 1827 1828 if( !invert ) 1829 outPlanes[ numPlanes ] = PlaneF( v1, fromPoint, v2 ); 1830 else 1831 outPlanes[ numPlanes ] = PlaneF( v2, fromPoint, v1 ); 1832 1833 numPlanes ++; 1834 1835 // If this is the first plane that we have created, find out whether 1836 // the vertex ordering is giving us the plane orientations that we want 1837 // (facing inside). If not, invert vertex order from now on. 1838 1839 if( i == 0 ) 1840 { 1841 const PlaneF& plane = outPlanes[ numPlanes - 1 ]; 1842 for( U32 n = i + 1; n < numVertices; ++ n ) 1843 { 1844 const PlaneF::Side side = plane.whichSide( vertices[ n ] ); 1845 if( side == PlaneF::On ) 1846 continue; 1847 1848 if( side != PlaneF::Front ) 1849 invert = true; 1850 break; 1851 } 1852 } 1853 } 1854 1855 return numPlanes; 1856} 1857 1858//----------------------------------------------------------------------------- 1859 1860void mBuildHull2D(const Vector<Point2F> _inPoints, Vector<Point2F> &hullPoints) 1861{ 1862 /// Andrew's monotone chain convex hull algorithm implementation 1863 1864 struct Util 1865 { 1866 //compare by x and then by y 1867 static int CompareLexicographic( const Point2F *a, const Point2F *b) 1868 { 1869 return a->x < b->x || (a->x == b->x && a->y < b->y); 1870 } 1871 }; 1872 1873 hullPoints.clear(); 1874 hullPoints.setSize( _inPoints.size()*2 ); 1875 1876 // sort in points by x and then by y 1877 Vector<Point2F> inSortedPoints = _inPoints; 1878 inSortedPoints.sort( &Util::CompareLexicographic ); 1879 1880 Point2F* lowerHullPtr = hullPoints.address(); 1881 U32 lowerHullIdx = 0; 1882 1883 //lower part of hull 1884 for( int i = 0; i < inSortedPoints.size(); ++i ) 1885 { 1886 while( lowerHullIdx >= 2 && mCross( lowerHullPtr[ lowerHullIdx - 2], lowerHullPtr[lowerHullIdx - 1], inSortedPoints[i] ) <= 0 ) 1887 --lowerHullIdx; 1888 1889 lowerHullPtr[lowerHullIdx++] = inSortedPoints[i]; 1890 } 1891 1892 --lowerHullIdx; // last point are the same as first in upperHullPtr 1893 1894 Point2F* upperHullPtr = hullPoints.address() + lowerHullIdx; 1895 U32 upperHullIdx = 0; 1896 1897 //upper part of hull 1898 for( int i = inSortedPoints.size()-1; i >= 0; --i ) 1899 { 1900 while( upperHullIdx >= 2 && mCross( upperHullPtr[ upperHullIdx - 2], upperHullPtr[upperHullIdx - 1], inSortedPoints[i] ) <= 0 ) 1901 --upperHullIdx; 1902 1903 upperHullPtr[upperHullIdx++] = inSortedPoints[i]; 1904 } 1905 1906 hullPoints.setSize( lowerHullIdx + upperHullIdx ); 1907} 1908 1909} // namespace MathUtils 1910