File: Intersect.cpp

package info (click to toggle)
mercator 0.3.0-2
  • links: PTS, VCS
  • area: main
  • in suites: wheezy
  • size: 2,008 kB
  • sloc: sh: 10,433; cpp: 4,482; makefile: 115
file content (300 lines) | stat: -rw-r--r-- 9,779 bytes parent folder | download | duplicates (5)
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