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