Extras/CDTestFramework/IceHelpers.cpp

   1 /*
   2 CDTestFramework http://codercorner.com
   3 Copyright (c) 2007-2008 Pierre Terdiman,  pierre@codercorner.com
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "stdafx.h"
  17 #include "IceHelpers.h"
  18
  19 // Misc functions borrowed & adapted from ICE
  20
  21 void RotX(Matrix3x3& m, float angle)
  22 {
  23         float Cos = cosf(angle);
  24         float Sin = sinf(angle);
  25         m.Identity();
  26         m.m[1][1] = m.m[2][2] = Cos;
  27         m.m[2][1] = -Sin;
  28         m.m[1][2] = Sin;
  29 }
  30
  31 void RotY(Matrix3x3& m, float angle)
  32 {
  33         float Cos = cosf(angle);
  34         float Sin = sinf(angle);
  35         m.Identity();
  36         m.m[0][0] = m.m[2][2] = Cos;
  37         m.m[2][0] = Sin;
  38         m.m[0][2] = -Sin;
  39 }
  40
  41 void RotZ(Matrix3x3& m, float angle)
  42 {
  43         float Cos = cosf(angle);
  44         float Sin = sinf(angle);
  45         m.Identity();
  46         m.m[0][0] = m.m[1][1] = Cos;
  47         m.m[1][0] = -Sin;
  48         m.m[0][1] = Sin;
  49 }
  50
  51 bool SegmentSphere(const Point& origin, const Point& dir, float length, const Point& center, float radius, float& dist, Point& hit_pos)
  52 {
  53         // get the offset vector
  54         Point offset = center - origin;
  55
  56         // get the distance along the ray to the center point of the sphere
  57         float ray_dist = dir | offset;
  58
  59         // get the squared distances
  60         float off2 = offset | offset;
  61         float rd2 = radius * radius;
  62         if(off2 <= rd2)
  63         {
  64                 // we're in the sphere
  65                 hit_pos = origin;
  66                 dist    = 0.0f;
  67                 return true;
  68         }
  69
  70         if(ray_dist <= 0 || (ray_dist - length) > radius)
  71         {
  72                 // moving away from object or too far away
  73                 return false;
  74         }
  75
  76         // find hit distance squared
  77         float d = rd2 - (off2 - ray_dist * ray_dist);
  78         if(d<0.0f)
  79         {
  80                 // ray passes by sphere without hitting
  81                 return false;
  82         }
  83
  84         // get the distance along the ray
  85         dist = ray_dist - sqrtf(d);
  86         if(dist > length)
  87         {
  88                 // hit point beyond length
  89                 return false;
  90         }
  91
  92         // sort out the details
  93         hit_pos = origin + dir * dist;
  94         return true;
  95 }
  96
  97 bool /*Ctc::*/RayAABB2(const Point& min, const Point& max, const Point& origin, const Point& dir, Point& coord)
  98 {
  99         BOOL Inside = TRUE;
 100         Point MaxT;
 101         MaxT.x=MaxT.y=MaxT.z=-1.0f;
 102
 103         // Find candidate planes.
 104         for(udword i=0;i<3;i++)
 105         {
 106                 if(origin[i] < min[i])
 107                 {
 108                         coord[i]        = min[i];
 109                         Inside          = FALSE;
 110
 111                         // Calculate T distances to candidate planes
 112                         if(IR(dir[i]))  MaxT[i] = (min[i] - origin[i]) / dir[i];
 113                 }
 114                 else if(origin[i] > max[i])
 115                 {
 116                         coord[i]        = max[i];
 117                         Inside          = FALSE;
 118
 119                         // Calculate T distances to candidate planes
 120                         if(IR(dir[i]))  MaxT[i] = (max[i] - origin[i]) / dir[i];
 121                 }
 122         }
 123
 124         // Ray origin inside bounding box
 125         if(Inside)
 126         {
 127                 coord = origin;
 128                 return true;
 129         }
 130
 131         // Get largest of the maxT's for final choice of intersection
 132         udword WhichPlane = 0;
 133         if(MaxT[1] > MaxT[WhichPlane])  WhichPlane = 1;
 134         if(MaxT[2] > MaxT[WhichPlane])  WhichPlane = 2;
 135
 136         // Check final candidate actually inside box
 137         if(IR(MaxT[WhichPlane])&0x80000000) return false;
 138
 139         for(udword i=0;i<3;i++)
 140         {
 141                 if(i!=WhichPlane)
 142                 {
 143                         coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
 144 #ifdef RAYAABB_EPSILON
 145                         if(coord[i] < min[i] - RAYAABB_EPSILON || coord[i] > max[i] + RAYAABB_EPSILON)  return false;
 146 #else
 147                         if(coord[i] < min[i] || coord[i] > max[i])      return false;
 148 #endif
 149                 }
 150         }
 151         return true;    // ray hits box
 152 }
 153
 154 static const bool gNormalize = true;
 155
 156 udword /*Ctc::*/RayCapsuleOverlap(const Point& origin, const Point& dir, const LSS& capsule, float s[2])
 157 {
 158         // set up quadratic Q(t) = a*t^2 + 2*b*t + c
 159         Point kU, kV, kW, capsDir;
 160         capsule.ComputeDirection(capsDir);
 161         kW = capsDir;
 162
 163         float fWLength = kW.Magnitude();
 164         kW.Normalize();
 165
 166         // generate orthonormal basis
 167
 168     float fInvLength;
 169         if ( fabsf(kW.x) >= fabsf(kW.y) )
 170     {
 171         // W.x or W.z is the largest magnitude component, swap them
 172                 fInvLength = 1.0f/sqrtf(kW.x*kW.x + kW.z*kW.z);
 173         kU.x = -kW.z*fInvLength;
 174         kU.y = 0.0f;
 175         kU.z = +kW.x*fInvLength;
 176     }
 177     else
 178     {
 179         // W.y or W.z is the largest magnitude component, swap them
 180         fInvLength = 1.0f/sqrtf(kW.y*kW.y + kW.z*kW.z);
 181         kU.x = 0.0f;
 182         kU.y = +kW.z*fInvLength;
 183         kU.z = -kW.y*fInvLength;
 184     }
 185     kV = kW^kU;
 186 kU.Normalize();
 187         if(gNormalize)
 188         kV.Normalize();
 189
 190         // compute intersection
 191
 192         Point kD(kU|dir, kV|dir, kW|dir);
 193         float fDLength = kD.Magnitude();
 194         kD.Normalize();
 195
 196         float fInvDLength = 1.0f/fDLength;
 197         Point kDiff = origin - capsule.mP0;
 198         Point kP(kU|kDiff, kV|kDiff, kW|kDiff);
 199         float fRadiusSqr = capsule.mRadius*capsule.mRadius;
 200
 201         float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;
 202
 203         // Is the velocity parallel to the capsule direction? (or zero)
 204         if ( fabsf(kD.z) >= 1.0f - FLT_EPSILON || fDLength < FLT_EPSILON )
 205                 {
 206
 207                 float fAxisDir = dir|capsDir;
 208
 209                 fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
 210                 if ( fAxisDir < 0 && fDiscr >= 0.0f )
 211                         {
 212                         // Velocity anti-parallel to the capsule direction
 213                         fRoot = sqrtf(fDiscr);
 214                         s[0] = (kP.z + fRoot)*fInvDLength;
 215                         s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
 216                         return 2;
 217                         }
 218                 else if ( fAxisDir > 0  && fDiscr >= 0.0f )
 219                         {
 220                         // Velocity parallel to the capsule direction
 221                         fRoot = sqrtf(fDiscr);
 222                         s[0] = -(kP.z + fRoot)*fInvDLength;
 223                         s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
 224                         return 2;
 225                         }
 226                 else
 227                         {
 228                         // sphere heading wrong direction, or no velocity at all
 229                         return 0;
 230                         }
 231                 }
 232
 233         // test intersection with infinite cylinder
 234         fA = kD.x*kD.x + kD.y*kD.y;
 235         fB = kP.x*kD.x + kP.y*kD.y;
 236         fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
 237         fDiscr = fB*fB - fA*fC;
 238         if ( fDiscr < 0.0f )
 239                 {
 240                 // line does not intersect infinite cylinder
 241                 return 0;
 242                 }
 243
 244         int iQuantity = 0;
 245
 246         if ( fDiscr > 0.0f )
 247                 {
 248                 // line intersects infinite cylinder in two places
 249                 fRoot = sqrtf(fDiscr);
 250                 fInv = 1.0f/fA;
 251                 fT = (-fB - fRoot)*fInv;
 252                 fTmp = kP.z + fT*kD.z;
 253                 if ( 0.0f <= fTmp && fTmp <= fWLength )
 254                         s[iQuantity++] = fT*fInvDLength;
 255
 256                 fT = (-fB + fRoot)*fInv;
 257                 fTmp = kP.z + fT*kD.z;
 258                 if ( 0.0f <= fTmp && fTmp <= fWLength )
 259                         s[iQuantity++] = fT*fInvDLength;
 260
 261                 if ( iQuantity == 2 )
 262                         {
 263                         // line intersects capsule wall in two places
 264                         return 2;
 265                         }
 266                 }
 267         else
 268                 {
 269                 // line is tangent to infinite cylinder
 270                 fT = -fB/fA;
 271                 fTmp = kP.z + fT*kD.z;
 272                 if ( 0.0f <= fTmp && fTmp <= fWLength )
 273                         {
 274                         s[0] = fT*fInvDLength;
 275                         return 1;
 276                         }
 277                 }
 278
 279         // test intersection with bottom hemisphere
 280         // fA = 1
 281         fB += kP.z*kD.z;
 282         fC += kP.z*kP.z;
 283         fDiscr = fB*fB - fC;
 284         if ( fDiscr > 0.0f )
 285                 {
 286                 fRoot = sqrtf(fDiscr);
 287                 fT = -fB - fRoot;
 288                 fTmp = kP.z + fT*kD.z;
 289                 if ( fTmp <= 0.0f )
 290                         {
 291                         s[iQuantity++] = fT*fInvDLength;
 292                         if ( iQuantity == 2 )
 293                                 return 2;
 294                         }
 295
 296                 fT = -fB + fRoot;
 297                 fTmp = kP.z + fT*kD.z;
 298                 if ( fTmp <= 0.0f )
 299                         {
 300                         s[iQuantity++] = fT*fInvDLength;
 301                         if ( iQuantity == 2 )
 302                                 return 2;
 303                         }
 304                 }
 305         else if ( fDiscr == 0.0f )
 306                 {
 307                 fT = -fB;
 308                 fTmp = kP.z + fT*kD.z;
 309                 if ( fTmp <= 0.0f )
 310                         {
 311                         s[iQuantity++] = fT*fInvDLength;
 312                         if ( iQuantity == 2 )
 313                                 return 2;
 314                         }
 315                 }
 316
 317         // test intersection with top hemisphere
 318         // fA = 1
 319         fB -= kD.z*fWLength;
 320         fC += fWLength*(fWLength - 2.0f*kP.z);
 321
 322         fDiscr = fB*fB - fC;
 323         if ( fDiscr > 0.0f )
 324                 {
 325                 fRoot = sqrtf(fDiscr);
 326                 fT = -fB - fRoot;
 327                 fTmp = kP.z + fT*kD.z;
 328                 if ( fTmp >= fWLength )
 329                         {
 330                         s[iQuantity++] = fT*fInvDLength;
 331                         if ( iQuantity == 2 )
 332                                 return 2;
 333                         }
 334
 335                 fT = -fB + fRoot;
 336                 fTmp = kP.z + fT*kD.z;
 337                 if ( fTmp >= fWLength )
 338                         {
 339                         s[iQuantity++] = fT*fInvDLength;
 340                         if ( iQuantity == 2 )
 341                                 return 2;
 342                         }
 343                 }
 344         else if ( fDiscr == 0.0f )
 345                 {
 346                 fT = -fB;
 347                 fTmp = kP.z + fT*kD.z;
 348                 if ( fTmp >= fWLength )
 349                         {
 350                         s[iQuantity++] = fT*fInvDLength;
 351                         if ( iQuantity == 2 )
 352                                 return 2;
 353                         }
 354                 }
 355
 356         return iQuantity;
 357 }
 358