Dogcows Code - chaz/yoink/blob - src/Moof/Line.hh

   1
   2 /*]  Copyright (c) 2009-2010, Charles McGarvey  [**************************
   3 **]  All rights reserved.
   4 *
   5 * vi:ts=4 sw=4 tw=75
   6 *
   7 * Distributable under the terms and conditions of the 2-clause BSD license;
   8 * see the file COPYING for a complete text of the license.
   9 *
  10 **************************************************************************/
  11
  12 #ifndef _MOOF_LINE_HH_
  13 #define _MOOF_LINE_HH_
  14
  15 #include <Moof/Contact.hh>
  16 #include <Moof/Drawable.hh>
  17 #include <Moof/Log.hh>
  18 #include <Moof/Math.hh>
  19 #include <Moof/OpenGL.hh>
  20 #include <Moof/Ray.hh>
  21 #include <Moof/Shape.hh>
  22 #include <Moof/Sphere.hh>
  23 #include <Moof/Texture.hh>
  24
  25
  26 namespace Mf {
  27
  28
  29 template <int D>
  30 struct Line : public Drawable, public Shape<D>
  31 {
  32         typedef cml::vector< Scalar, cml::fixed<D> > Vector;
  33
  34         Vector  a;
  35         Vector  b;
  36
  37
  38         Line() {}
  39
  40         Line(const Vector& point1, const Vector& point2) :
  41                 a(point1),
  42                 b(point2) {}
  43
  44
  45         Vector getDirection() const
  46         {
  47                 return b - a;
  48         }
  49
  50         Scalar getLength() const
  51         {
  52                 return getDirection().length();
  53         }
  54
  55
  56         bool intersect(const Line& other, Contact<D>& hit) const
  57         {
  58                 Scalar d = (other.b[1] - other.a[1]) * (b[0] - a[0]) -
  59                                    (other.b[0] - other.a[0]) * (b[1] - a[1]);
  60
  61                 if (d == SCALAR(0.0)) return false;                     // lines are parallel
  62                 // ignoring the (somewhat remote) possibility of coincidence
  63
  64                 Scalar m = ((other.b[0] - other.a[0]) * (a[1] - other.a[1]) -
  65                                         (other.b[1] - other.a[1]) * (a[0] - other.a[0])) / d;
  66
  67                 Scalar n = ((b[0] - a[0]) * (b[1] - other.a[1]) -
  68                                         (b[1] - a[1]) * (b[0] - other.a[0])) / d;
  69
  70                 if (m < SCALAR(0.0) || m > SCALAR(1.0) ||       // not intersecting
  71                         n < SCALAR(0.0) || n > SCALAR(1.0)) return false;
  72
  73                 Vector2 tangent = b - a;
  74                 Vector2 normal = cml::perp(tangent).normalize();
  75
  76                 if (cml::dot(normal, other.a - other.b) < SCALAR(0.0))
  77                 {
  78                         normal = -normal;
  79                 }
  80
  81                 hit.point = a + m * tangent;
  82                 hit.normal = normal;
  83                 hit.distance = (other.b - hit.point).length();
  84
  85                 return true;
  86         }
  87
  88         bool intersect(const Sphere<D>& other, Contact<D>& hit) const
  89         {
  90                 Vector surface = b - a;
  91                 Vector toPoint = other.point - a;
  92
  93                 Scalar surfaceLength = surface.length();
  94                 surface.normalize();
  95
  96                 Scalar projection = cml::dot(surface, toPoint);
  97
  98                 if (projection < SCALAR(0.0) || projection > surfaceLength)
  99                 {
 100                         // try endpoints
 101
 102                         if (other.intersect(a, hit))
 103                         {
 104                                 hit.normal = -hit.normal;
 105                                 hit.point = a;
 106                                 return true;
 107                         }
 108                         else if (other.intersect(b, hit))
 109                         {
 110                                 hit.normal = -hit.normal;
 111                                 hit.point = b;
 112                                 return true;
 113                         }
 114
 115                         return false;
 116                 }
 117
 118                 Vector point = a + surface * projection;
 119                 Vector normal = other.point - point;
 120
 121                 Scalar distance = normal.length();
 122
 123                 if (distance > other.radius) false;             // not intersecting
 124
 125                 normal.normalize();
 126
 127                 hit.distance = other.radius - distance;
 128                 hit.point = point;
 129                 hit.normal = normal;
 130
 131                 return true;
 132         }
 133
 134
 135         bool intersectRay(const Ray<2>& ray, Ray<2>::Contact& hit) const
 136         {
 137                 Vector2 v1 = a - ray.point;
 138                 Scalar  a1 = cml::signed_angle_2D(v1, b - ray.point);
 139
 140                 //logWarning << "angle:::::::::: " << a1 << std::endl;
 141
 142                 if (a1 == Constants::pi())
 143                 {
 144                         hit.distance = 5.4321;
 145                         return true;
 146                 }
 147                 else if (a1 == SCALAR(0.0))
 148                 {
 149                         hit.distance = 99999.0;
 150                         return true;
 151                 }
 152
 153                 Scalar  a2 = cml::signed_angle_2D(v1, ray.direction);
 154
 155                 if (a2 < SCALAR(0.0) || a2 > a1) return false;
 156
 157                 //hit.distance = 1.23456;
 158                 //hit.normal = Vector2(0.0, 0.0);
 159
 160                 Vector2 n = (b - a).normalize();
 161                 Scalar  z = cml::dot(ray.point - a, n);
 162                 Vector2 p = a + n * z;
 163                 hit.distance = (ray.point - p).length();
 164                 hit.normal = cml::perp(a - b);
 165                 return true;
 166
 167
 168                 /*
 169                 // solve: Cx + r*Dx = Ax + s(Bx - Ax)
 170                 //        Cy + r*Dy = Ay + s(By - Ay)
 171                 // where: 0 <= s <= 1 if intersection
 172                 // given: A = a
 173                 //        B = b
 174                 //        C = ray.point
 175                 //        D = ray.direction
 176
 177                 Scalar denom = ray.direction[0] * (b[1] - a[1]) +
 178                                            ray.direction[1] * (a[0] - b[0]);
 179
 180                 // check if the ray and line are parallel
 181                 //if (isEqual(denom, SCALAR(0.0)))
 182                 if (denom == SCALAR(0.0))
 183                 {
 184                         Scalar numer = a[0] * (ray.point[1] - b[1]) +
 185                                                    b[0] * (a[1] - ray.point[1]) +
 186                                                    ray.point[0] * (b[1] - a[1]);
 187
 188                         // check if they are collinear
 189                         if (isEqual(numer, SCALAR(0.0)))
 190                         {
 191                                 hit.distance = SCALAR(0.0);
 192                                 hit.normal.set(0.0, 0.0);
 193                                 return true;
 194                         }
 195
 196                         return false;
 197                 }
 198
 199                 Scalar s = (ray.direction[0] * (ray.point[1] - a[1]) +
 200                                         ray.direction[1] * (a[0] - ray.point[0])) / denom;
 201
 202                 // check if the ray hits the segment
 203                 if (s < SCALAR(0.0) || s > SCALAR(1.0)) return false;
 204
 205                 hit.distance = -(a[0] * (ray.point[1] - b[1]) +
 206                                                  b[0] * (a[1] - ray.point[1]) +
 207                                                  ray.point[0] * (b[1] - a[1])) / denom;
 208
 209                 // check if the intersection is behind the ray
 210                 if (hit.distance < SCALAR(0.0)) return false;
 211
 212                 Vector normal = cml::perp(a - b);
 213                 if (cml::dot(a - ray.point, normal) < 0) hit.normal = normal;
 214                 else                                     hit.normal = -normal;
 215                 return true;
 216                 */
 217         }
 218
 219
 220         void draw(Scalar alpha = 0.0) const
 221         {
 222                 Mf::Texture::resetBind();
 223                 glBegin(GL_LINES);
 224                         glVertex(a);
 225                         glVertex(b);
 226                 glEnd();
 227         }
 228 };
 229
 230
 231 typedef Line<2>         Line2;
 232 typedef Line<3>         Line3;
 233
 234
 235 template <int D, int N>
 236 struct Polygon : public Drawable, public Shape<D>
 237 {
 238         typedef cml::vector< Scalar, cml::fixed<D> > Vector;
 239
 240         Vector  points[N];
 241
 242         Polygon() {}
 243
 244         bool intersectRay(const Ray<D>& ray, typename Ray<D>::Contact& hit)
 245         {
 246                 return false;
 247         }
 248
 249         void draw(Scalar alpha = 0.0) const
 250         {
 251                 Mf::Texture::resetBind();
 252                 glBegin(GL_POLYGON);
 253                 for (int i = 0; i < D; ++i)
 254                 {
 255                         glVertex(points[0]);
 256                 }
 257                 glEnd();
 258         }
 259 };
 260
 261
 262 typedef Polygon<2,3>    Triangle2;
 263 typedef Polygon<3,3>    Triangle3;
 264
 265
 266 template <int D>
 267 bool intersect(const Line<D>& line, const Sphere<D>& sphere,
 268                            Contact<D>& hit)
 269 {
 270         return false;
 271 }
 272
 273
 274 } // namespace Mf
 275
 276 #endif // _MOOF_LINE_HH_
 277