source/simulation2/helpers/Geometry.cpp

   1 /* Copyright (C) 2015 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * 0 A.D. is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "precompiled.h"
  19
  20 #include "Geometry.h"
  21
  22 using namespace Geometry;
  23
  24 // TODO: all of these things could be optimised quite easily
  25
  26 CFixedVector2D Geometry::GetHalfBoundingBox(const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize)
  27 {
  28         return CFixedVector2D(
  29                 u.X.Multiply(halfSize.X).Absolute() + v.X.Multiply(halfSize.Y).Absolute(),
  30                 u.Y.Multiply(halfSize.X).Absolute() + v.Y.Multiply(halfSize.Y).Absolute()
  31         );
  32 }
  33
  34 float Geometry::ChordToCentralAngle(const float chordLength, const float radius)
  35 {
  36         return acosf(1.f - SQR(chordLength)/(2.f*SQR(radius))); // cfr. law of cosines
  37 }
  38
  39 fixed Geometry::DistanceToSquare(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize, bool countInsideAsZero)
  40 {
  41         /*
  42          * Relative to its own coordinate system, we have a square like:
  43          *
  44          *  A  :    B    :  C
  45          *     :         :
  46          * - - ########### - -
  47          *     #         #
  48          *     # I       #
  49          *  D  #    0    #  E     v
  50          *     #         #        ^
  51          *     #         #        |
  52          * - - ########### - -      -->u
  53          *     :         :
  54          *  F  :    G    :  H
  55          *
  56          * where 0 is the center, u and v are unit axes,
  57          * and the square is hw*2 by hh*2 units in size.
  58          *
  59          * Points in the BIG regions should check distance to horizontal edges.
  60          * Points in the DIE regions should check distance to vertical edges.
  61          * Points in the ACFH regions should check distance to the corresponding corner.
  62          *
  63          * So we just need to check all of the regions to work out which calculations to apply.
  64          *
  65          */
  66
  67         // By symmetry (taking absolute values), we work only in the 0-B-C-E quadrant
  68         // du, dv are the location of the point in the square's coordinate system
  69         fixed du = point.Dot(u).Absolute();
  70         fixed dv = point.Dot(v).Absolute();
  71
  72         fixed hw = halfSize.X;
  73         fixed hh = halfSize.Y;
  74
  75         if (du < hw) // regions B, I, G
  76         {
  77                 if (dv < hh) // region I
  78                         return countInsideAsZero ? fixed::Zero() : std::min(hw - du, hh - dv);
  79                 else
  80                         return dv - hh;
  81         }
  82         else if (dv < hh) // regions D, E
  83         {
  84                 return du - hw; // vertical edges
  85         }
  86         else // regions A, C, F, H
  87         {
  88                 CFixedVector2D distance(du - hw, dv - hh);
  89                 return distance.Length();
  90         }
  91 }
  92
  93 // Same as above except it does not use Length
  94 // For explanations refer to DistanceToSquare
  95 fixed Geometry::DistanceToSquareSquared(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize, bool countInsideAsZero)
  96 {
  97         fixed du = point.Dot(u).Absolute();
  98         fixed dv = point.Dot(v).Absolute();
  99
 100         fixed hw = halfSize.X;
 101         fixed hh = halfSize.Y;
 102
 103         if (du < hw) // regions B, I, G
 104         {
 105                 if (dv < hh) // region I
 106                         return countInsideAsZero ? fixed::Zero() : std::min((hw - du).Square(), (hh - dv).Square());
 107                 else
 108                         return (dv - hh).Square(); // horizontal edges
 109         }
 110         else if (dv < hh) // regions D, E
 111         {
 112                 return (du - hw).Square(); // vertical edges
 113         }
 114         else // regions A, C, F, H
 115         {
 116                 return (du - hw).Square() + (dv - hh).Square();
 117         }
 118 }
 119
 120 CFixedVector2D Geometry::NearestPointOnSquare(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize)
 121 {
 122         /*
 123          * Relative to its own coordinate system, we have a square like:
 124          *
 125          *  A  :         :  C
 126          *     :         :
 127          * - - #### B #### - -
 128          *     #\       /#
 129          *     # \     / #
 130          *     D  --0--  E        v
 131          *     # /     \ #        ^
 132          *     #/       \#        |
 133          * - - #### G #### - -      -->u
 134          *     :         :
 135          *  F  :         :  H
 136          *
 137          * where 0 is the center, u and v are unit axes,
 138          * and the square is hw*2 by hh*2 units in size.
 139          *
 140          * Points in the BDEG regions are nearest to the corresponding edge.
 141          * Points in the ACFH regions are nearest to the corresponding corner.
 142          *
 143          * So we just need to check all of the regions to work out which calculations to apply.
 144          *
 145          */
 146
 147         // du, dv are the location of the point in the square's coordinate system
 148         fixed du = point.Dot(u);
 149         fixed dv = point.Dot(v);
 150
 151         fixed hw = halfSize.X;
 152         fixed hh = halfSize.Y;
 153
 154         if (-hw < du && du < hw) // regions B, G; or regions D, E inside the square
 155         {
 156                 if (-hh < dv && dv < hh && (du.Absolute() - hw).Absolute() < (dv.Absolute() - hh).Absolute()) // regions D, E
 157                 {
 158                         if (du >= fixed::Zero()) // E
 159                                 return u.Multiply(hw) + v.Multiply(dv);
 160                         else // D
 161                                 return -u.Multiply(hw) + v.Multiply(dv);
 162                 }
 163                 else // B, G
 164                 {
 165                         if (dv >= fixed::Zero()) // B
 166                                 return v.Multiply(hh) + u.Multiply(du);
 167                         else // G
 168                                 return -v.Multiply(hh) + u.Multiply(du);
 169                 }
 170         }
 171         else if (-hh < dv && dv < hh) // regions D, E outside the square
 172         {
 173                 if (du >= fixed::Zero()) // E
 174                         return u.Multiply(hw) + v.Multiply(dv);
 175                 else // D
 176                         return -u.Multiply(hw) + v.Multiply(dv);
 177         }
 178         else // regions A, C, F, H
 179         {
 180                 CFixedVector2D corner;
 181                 if (du < fixed::Zero()) // A, F
 182                         corner -= u.Multiply(hw);
 183                 else // C, H
 184                         corner += u.Multiply(hw);
 185                 if (dv < fixed::Zero()) // F, H
 186                         corner -= v.Multiply(hh);
 187                 else // A, C
 188                         corner += v.Multiply(hh);
 189
 190                 return corner;
 191         }
 192 }
 193
 194 bool Geometry::TestRaySquare(const CFixedVector2D& a, const CFixedVector2D& b, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize)
 195 {
 196         /*
 197          * We only consider collisions to be when the ray goes from outside to inside the shape (and possibly out again).
 198          * Various cases to consider:
 199          *   'a' inside, 'b' inside -> no collision
 200          *   'a' inside, 'b' outside -> no collision
 201          *   'a' outside, 'b' inside -> collision
 202          *   'a' outside, 'b' outside -> depends; use separating axis theorem:
 203          *     if the ray's bounding box is outside the square -> no collision
 204          *     if the whole square is on the same side of the ray -> no collision
 205          *     otherwise -> collision
 206          * (Points on the edge are considered 'inside'.)
 207          */
 208
 209         fixed hw = halfSize.X;
 210         fixed hh = halfSize.Y;
 211
 212         fixed au = a.Dot(u);
 213         fixed av = a.Dot(v);
 214
 215         if (-hw <= au && au <= hw && -hh <= av && av <= hh)
 216                 return false; // a is inside
 217
 218         fixed bu = b.Dot(u);
 219         fixed bv = b.Dot(v);
 220
 221         if (-hw <= bu && bu <= hw && -hh <= bv && bv <= hh) // TODO: isn't this subsumed by the next checks?
 222                 return true; // a is outside, b is inside
 223
 224         if ((au < -hw && bu < -hw) || (au > hw && bu > hw) || (av < -hh && bv < -hh) || (av > hh && bv > hh))
 225                 return false; // ab is entirely above/below/side the square
 226
 227         CFixedVector2D abp = (b - a).Perpendicular();
 228         fixed s0 = abp.Dot((u.Multiply(hw) + v.Multiply(hh)) - a);
 229         fixed s1 = abp.Dot((u.Multiply(hw) - v.Multiply(hh)) - a);
 230         fixed s2 = abp.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a);
 231         fixed s3 = abp.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a);
 232         if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero())
 233                 return true; // ray intersects the corner
 234
 235         bool sign = (s0 < fixed::Zero());
 236         if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign)
 237                 return true; // ray cuts through the square
 238
 239         return false;
 240 }
 241
 242 // Exactly like TestRaySquare with u=(1,0), v=(0,1)
 243 bool Geometry::TestRayAASquare(const CFixedVector2D& a, const CFixedVector2D& b, const CFixedVector2D& halfSize)
 244 {
 245         fixed hw = halfSize.X;
 246         fixed hh = halfSize.Y;
 247
 248         if (-hw <= a.X && a.X <= hw && -hh <= a.Y && a.Y <= hh)
 249                 return false; // a is inside
 250
 251         if (-hw <= b.X && b.X <= hw && -hh <= b.Y && b.Y <= hh) // TODO: isn't this subsumed by the next checks?
 252                 return true; // a is outside, b is inside
 253
 254         if ((a.X < -hw && b.X < -hw) || (a.X > hw && b.X > hw) || (a.Y < -hh && b.Y < -hh) || (a.Y > hh && b.Y > hh))
 255                 return false; // ab is entirely above/below/side the square
 256
 257         CFixedVector2D abp = (b - a).Perpendicular();
 258         fixed s0 = abp.Dot(CFixedVector2D(hw, hh) - a);
 259         fixed s1 = abp.Dot(CFixedVector2D(hw, -hh) - a);
 260         fixed s2 = abp.Dot(CFixedVector2D(-hw, -hh) - a);
 261         fixed s3 = abp.Dot(CFixedVector2D(-hw, hh) - a);
 262         if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero())
 263                 return true; // ray intersects the corner
 264
 265         bool sign = (s0 < fixed::Zero());
 266         if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign)
 267                 return true; // ray cuts through the square
 268
 269         return false;
 270 }
 271
 272 /**
 273  * Separating axis test; returns true if the square defined by u/v/halfSize at the origin
 274  * is not entirely on the clockwise side of a line in direction 'axis' passing through 'a'
 275  */
 276 static bool SquareSAT(const CFixedVector2D& a, const CFixedVector2D& axis, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize)
 277 {
 278         fixed hw = halfSize.X;
 279         fixed hh = halfSize.Y;
 280
 281         CFixedVector2D p = axis.Perpendicular();
 282         if (p.Dot((u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero())
 283                 return true;
 284         if (p.Dot((u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero())
 285                 return true;
 286         if (p.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero())
 287                 return true;
 288         if (p.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero())
 289                 return true;
 290
 291         return false;
 292 }
 293
 294 bool Geometry::TestSquareSquare(
 295                 const CFixedVector2D& c0, const CFixedVector2D& u0, const CFixedVector2D& v0, const CFixedVector2D& halfSize0,
 296                 const CFixedVector2D& c1, const CFixedVector2D& u1, const CFixedVector2D& v1, const CFixedVector2D& halfSize1)
 297 {
 298         // TODO: need to test this carefully
 299
 300         CFixedVector2D corner0a = c0 + u0.Multiply(halfSize0.X) + v0.Multiply(halfSize0.Y);
 301         CFixedVector2D corner0b = c0 - u0.Multiply(halfSize0.X) - v0.Multiply(halfSize0.Y);
 302         CFixedVector2D corner1a = c1 + u1.Multiply(halfSize1.X) + v1.Multiply(halfSize1.Y);
 303         CFixedVector2D corner1b = c1 - u1.Multiply(halfSize1.X) - v1.Multiply(halfSize1.Y);
 304
 305         // Do a SAT test for each square vs each edge of the other square
 306         if (!SquareSAT(corner0a - c1, -u0, u1, v1, halfSize1))
 307                 return false;
 308         if (!SquareSAT(corner0a - c1, v0, u1, v1, halfSize1))
 309                 return false;
 310         if (!SquareSAT(corner0b - c1, u0, u1, v1, halfSize1))
 311                 return false;
 312         if (!SquareSAT(corner0b - c1, -v0, u1, v1, halfSize1))
 313                 return false;
 314         if (!SquareSAT(corner1a - c0, -u1, u0, v0, halfSize0))
 315                 return false;
 316         if (!SquareSAT(corner1a - c0, v1, u0, v0, halfSize0))
 317                 return false;
 318         if (!SquareSAT(corner1b - c0, u1, u0, v0, halfSize0))
 319                 return false;
 320         if (!SquareSAT(corner1b - c0, -v1, u0, v0, halfSize0))
 321                 return false;
 322
 323         return true;
 324 }