1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
|
// This file may be redistributed and modified only under the terms of
// the GNU General Public License (See COPYING for details).
// Copyright (C) 2003 Damien McGinnes
#include <Mercator/Intersect.h>
#include <Mercator/Segment.h>
namespace Mercator {
//floor and ceil functions that return d-1 and d+1
//respectively if d is integral
static inline float gridceil(float d)
{
float c = ceil(d);
return (c==d) ? c+1.0f : c;
}
static inline float gridfloor(float d)
{
float c = floor(d);
return (c==d) ? c-1.0f : c;
}
//check intersection of an axis-aligned box with the terrain
bool Intersect(const Terrain &t, const WFMath::AxisBox<3> &bbox)
{
float max, min=bbox.lowCorner()[2];
int res = t.getResolution();
//determine which segments are involved
//usually will just be one
int xlow = (int) floor(bbox.lowCorner()[0] / res);
int xhigh = (int) gridceil(bbox.highCorner()[0] / res);
int ylow = (int) floor(bbox.lowCorner()[1] / res);
int yhigh = (int) gridceil(bbox.highCorner()[1] / res);
//loop across all tiles covered by this bbox
for (int x = xlow; x < xhigh; x++) {
for (int y = ylow; y < yhigh; y++) {
//check the bbox against the extent of each tile
//as an early rejection
Segment *thisSeg=t.getSegment(x,y);
if (thisSeg)
max=thisSeg->getMax();
else
max=Terrain::defaultLevel;
if (max > min) {
//entity bbox overlaps with the extents of this tile
//now check each tile point covered by the entity bbox
//clip the points to be tested against the bbox
int min_x = (int) floor(bbox.lowCorner()[0] - (x * res));
if (min_x < 0) min_x = 0;
int max_x = (int) gridceil(bbox.highCorner()[0] - (x * res));
if (max_x > res) min_x = res;
int min_y = (int) floor(bbox.lowCorner()[1] - (y * res));
if (min_y < 0) min_y = 0;
int max_y = (int) gridceil(bbox.highCorner()[1] - (y * res));
if (max_y > res) min_y = res;
//loop over each point and see if it is greater than the minimum
//of the bbox. If all points are below, the the bbox does NOT
//intersect. If a single point is above, then the bbox MIGHT
//intersect.
for (int xpt = min_x; xpt <= max_x; xpt++) {
for (int ypt = min_y; ypt <= max_y; ypt++) {
if (thisSeg) {
if (thisSeg->get(xpt,ypt) > min) return true;
}
else if (Terrain::defaultLevel > min) return true;
}
}
}
}
}
return false;
}
bool Intersect(const Terrain &t, const WFMath::Point<3> &pt)
{
return HOT(t, pt) <= 0.0;
}
float HOT(const Terrain &t, const WFMath::Point<3> &pt)
{
WFMath::Vector<3> normal;
float terrHeight;
t.getHeightAndNormal(pt[0], pt[1], terrHeight, normal);
return (pt[2] - terrHeight);
}
//helper function for ray terrain intersection
static bool cellIntersect(float h1, float h2, float h3, float h4, float X, float Y,
const WFMath::Vector<3> &nDir, float dirLen,
const WFMath::Point<3> &sPt, WFMath::Point<3> &intersection,
WFMath::Vector<3> &normal, float &par)
{
//ray plane intersection roughly using the following:
//parametric ray eqn: p=po + par V
//plane eqn: p dot N + d = 0
//
//intersection:
// -par = (po dot N + d ) / (V dot N)
//
//
// effectively we calculate the ray parametric variable for the
// intersection of the plane corresponding to each triangle
// then clip them by endpints of the ray, and by the sides of the square
// and by the diagonal
//
// if they both still intersect, then we choose the earlier intersection
//intersection points for top and bottom triangles
WFMath::Point<3> topInt, botInt;
//point to use in plane equation for both triangles
WFMath::Vector<3> p0 = WFMath::Vector<3>(X, Y, h1);
// square is broken into two triangles
// top triangle |/
bool topIntersected = false;
WFMath::Vector<3> topNormal(h2-h3, h1-h2, 1.0);
topNormal.normalize();
float t = Dot(nDir, topNormal);
float topP=0.0;
if ((t > 1e-7) || (t < -1e-7)) {
topP = - (Dot((sPt-WFMath::Point<3>(0.,0.,0.)), topNormal)
- Dot(topNormal, p0)) / t;
topInt = sPt + nDir*topP;
//check the intersection is inside the triangle, and within the ray extents
if ((topP <= dirLen) && (topP > 0.0) &&
(topInt[0] >= X ) && (topInt[1] <= Y + 1 ) &&
((topInt[0] - topInt[1]) <= (X - Y)) ) {
topIntersected=true;
}
}
// bottom triangle /|
bool botIntersected = false;
WFMath::Vector<3> botNormal(h1-h4, h4-h3, 1.0);
botNormal.normalize();
float b = Dot(nDir, botNormal);
float botP=0.0;
if ((b > 1e-7) || (b < -1e-7)) {
botP = - (Dot((sPt-WFMath::Point<3>(0.,0.,0.)), botNormal)
- Dot(botNormal, p0)) / b;
botInt = sPt + nDir*botP;
//check the intersection is inside the triangle, and within the ray extents
if ((botP <= dirLen) && (botP > 0.0) &&
(botInt[0] <= X + 1 ) && (botInt[1] >= Y ) &&
((botInt[0] - botInt[1]) >= (X - Y)) ) {
botIntersected = true;
}
}
if (topIntersected && botIntersected) { //intersection with both
if (botP <= topP) {
intersection = botInt;
normal = botNormal;
par=botP/dirLen;
if (botP == topP) {
normal += topNormal;
normal.normalize();
}
return true;
}
else {
intersection = topInt;
normal = topNormal;
par=topP/dirLen;
return true;
}
}
else if (topIntersected) { //intersection with top
intersection = topInt;
normal = topNormal;
par=topP/dirLen;
return true;
}
else if (botIntersected) { //intersection with bot
intersection = botInt;
normal = botNormal;
par=botP/dirLen;
return true;
}
return false;
}
//Intersection of ray with terrain
//
//returns true on successful intersection
//ray is represented by a start point (sPt)
//and a direction vector (dir). The length of dir is
//used as the length of the ray. (ie not an infinite ray)
//intersection and normal are filled in on a successful result.
//par is the distance along the ray where intersection occurs
//0.0 == start, 1.0 == end.
bool Intersect(const Terrain &t, const WFMath::Point<3> &sPt, const WFMath::Vector<3>& dir,
WFMath::Point<3> &intersection, WFMath::Vector<3> &normal, float &par)
{
//FIXME early reject using segment max
//FIXME handle height point getting in a more optimal way
//FIXME early reject for vertical ray
// check if startpoint is below ground
if (HOT(t, sPt) < 0.0) return true;
float paraX=0.0, paraY=0.0; //used to store the parametric gap between grid crossings
float pX, pY; //the accumulators for the parametrics as we traverse the ray
float h1,h2,h3,h4,height;
WFMath::Point<3> last(sPt), next(sPt);
WFMath::Vector<3> nDir(dir);
nDir.normalize();
float dirLen = dir.mag();
//work out where the ray first crosses an X grid line
if (dir[0] != 0.0f) {
paraX = 1.0f/dir[0];
float crossX = (dir[0] > 0.0f) ? gridceil(last[0]) : gridfloor(last[0]);
pX = (crossX - last[0]) * paraX;
pX = std::min(pX, 1.0f);
}
else { //parallel: never crosses
pX = 1.0f;
}
//work out where the ray first crosses a Y grid line
if (dir[1] != 0.0f) {
paraY = 1.0f/dir[1];
float crossY = (dir[1] > 0.0f) ? gridceil(last[1]) : gridfloor(last[1]);
pY = (crossY - sPt[1]) * paraY;
pY = std::min(pY, 1.0f);
}
else { //parallel: never crosses
pY = 1.0f;
}
//ensure we traverse the ray forwards
paraX = std::abs(paraX);
paraY = std::abs(paraY);
bool endpoint = false;
//check each candidate tile for an intersection
while (1) {
last = next;
if (pX < pY) { // cross x grid line first
next = sPt + (pX * dir);
pX += paraX; // set x accumulator to current p
}
else { //cross y grid line first
next = sPt + (pY * dir);
if (pX == pY) {
pX += paraX; //unusual case where ray crosses corner
}
pY += paraY; // set y accumulator to current p
}
//FIXME these gets could be optimized a bit
float x= (dir[0] > 0) ? floor(last[0]) : floor(next[0]);
float y= (dir[1] > 0) ? floor(last[1]) : floor(next[1]);
h1 = t.get(x, y);
h2 = t.get(x, y+1);
h3 = t.get(x+1, y+1);
h4 = t.get(x+1, y);
height = std::max( std::max(h1, h2),
std::max(h3, h4));
if ( (last[2] < height) || (next[2] < height) ) {
// possible intersect with this tile
if (cellIntersect(h1, h2, h3, h4, x, y, nDir, dirLen, sPt,
intersection, normal, par)) {
return true;
}
}
if ((pX >= 1.0f) && (pY >= 1.0f)) {
if (endpoint) {
break;
}
else endpoint = true;
}
}
return false;
}
} // namespace Mercator
|