← Back to branch summary

~ubuntu-branches/debian/jessie/libccd/jessie

« back to all changes in this revision

Viewing changes to src/polytope.c

  • Committer: Package Import Robot
  • Author(s): Jose Luis Rivero
  • Date: 2014-03-24 16:51:48 UTC
  • Revision ID: package-import@ubuntu.com-20140324165148-mfno979f58rv322z
Tags: upstream-2.0
Import upstream version 2.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/polytope.c
 
1
/***
 
2
 * libccd
 
3
 * ---------------------------------
 
4
 * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
 
5
 *
 
6
 *
 
7
 *  This file is part of libccd.
 
8
 *
 
9
 *  Distributed under the OSI-approved BSD License (the "License");
 
10
 *  see accompanying file BDS-LICENSE for details or see
 
11
 *  <http://www.opensource.org/licenses/bsd-license.php>.
 
12
 *
 
13
 *  This software is distributed WITHOUT ANY WARRANTY; without even the
 
14
 *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 
15
 *  See the License for more information.
 
16
 */
 
17
 
 
18
#include <stdio.h>
 
19
#include <float.h>
 
20
#include "polytope.h"
 
21
#include "alloc.h"
 
22
 
 
23
_ccd_inline void _ccdPtNearestUpdate(ccd_pt_t *pt, ccd_pt_el_t *el)
 
24
{
 
25
    if (ccdEq(pt->nearest_dist, el->dist)){
 
26
        if (el->type < pt->nearest_type){
 
27
            pt->nearest = el;
 
28
            pt->nearest_dist = el->dist;
 
29
            pt->nearest_type = el->type;
 
30
        }
 
31
    }else if (el->dist < pt->nearest_dist){
 
32
        pt->nearest = el;
 
33
        pt->nearest_dist = el->dist;
 
34
        pt->nearest_type = el->type;
 
35
    }
 
36
}
 
37
 
 
38
static void _ccdPtNearestRenew(ccd_pt_t *pt)
 
39
{
 
40
    ccd_pt_vertex_t *v;
 
41
    ccd_pt_edge_t *e;
 
42
    ccd_pt_face_t *f;
 
43
 
 
44
    pt->nearest_dist = CCD_REAL_MAX;
 
45
    pt->nearest_type = 3;
 
46
    pt->nearest = NULL;
 
47
 
 
48
    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
 
49
        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)v);
 
50
    }
 
51
 
 
52
    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
 
53
        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)e);
 
54
    }
 
55
 
 
56
    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
 
57
        _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)f);
 
58
    }
 
59
}
 
60
 
 
61
 
 
62
 
 
63
void ccdPtInit(ccd_pt_t *pt)
 
64
{
 
65
    ccdListInit(&pt->vertices);
 
66
    ccdListInit(&pt->edges);
 
67
    ccdListInit(&pt->faces);
 
68
 
 
69
    pt->nearest = NULL;
 
70
    pt->nearest_dist = CCD_REAL_MAX;
 
71
    pt->nearest_type = 3;
 
72
}
 
73
 
 
74
void ccdPtDestroy(ccd_pt_t *pt)
 
75
{
 
76
    ccd_pt_face_t *f, *f2;
 
77
    ccd_pt_edge_t *e, *e2;
 
78
    ccd_pt_vertex_t *v, *v2;
 
79
 
 
80
    // first delete all faces
 
81
    ccdListForEachEntrySafe(&pt->faces, f, ccd_pt_face_t, f2, ccd_pt_face_t, list){
 
82
        ccdPtDelFace(pt, f);
 
83
    }
 
84
 
 
85
    // delete all edges
 
86
    ccdListForEachEntrySafe(&pt->edges, e, ccd_pt_edge_t, e2, ccd_pt_edge_t, list){
 
87
        ccdPtDelEdge(pt, e);
 
88
    }
 
89
 
 
90
    // delete all vertices
 
91
    ccdListForEachEntrySafe(&pt->vertices, v, ccd_pt_vertex_t, v2, ccd_pt_vertex_t, list){
 
92
        ccdPtDelVertex(pt, v);
 
93
    }
 
94
}
 
95
 
 
96
 
 
97
ccd_pt_vertex_t *ccdPtAddVertex(ccd_pt_t *pt, const ccd_support_t *v)
 
98
{
 
99
    ccd_pt_vertex_t *vert;
 
100
 
 
101
    vert = CCD_ALLOC(ccd_pt_vertex_t);
 
102
    if (vert == NULL)
 
103
        return NULL;
 
104
 
 
105
    vert->type = CCD_PT_VERTEX;
 
106
    ccdSupportCopy(&vert->v, v);
 
107
 
 
108
    vert->dist = ccdVec3Len2(&vert->v.v);
 
109
    ccdVec3Copy(&vert->witness, &vert->v.v);
 
110
 
 
111
    ccdListInit(&vert->edges);
 
112
 
 
113
    // add vertex to list
 
114
    ccdListAppend(&pt->vertices, &vert->list);
 
115
 
 
116
    // update position in .nearest array
 
117
    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)vert);
 
118
 
 
119
    return vert;
 
120
}
 
121
 
 
122
ccd_pt_edge_t *ccdPtAddEdge(ccd_pt_t *pt, ccd_pt_vertex_t *v1,
 
123
                                          ccd_pt_vertex_t *v2)
 
124
{
 
125
    const ccd_vec3_t *a, *b;
 
126
    ccd_pt_edge_t *edge;
 
127
 
 
128
    if (v1 == NULL || v2 == NULL)
 
129
        return NULL;
 
130
 
 
131
    edge = CCD_ALLOC(ccd_pt_edge_t);
 
132
    if (edge == NULL)
 
133
        return NULL;
 
134
 
 
135
    edge->type = CCD_PT_EDGE;
 
136
    edge->vertex[0] = v1;
 
137
    edge->vertex[1] = v2;
 
138
    edge->faces[0] = edge->faces[1] = NULL;
 
139
 
 
140
    a = &edge->vertex[0]->v.v;
 
141
    b = &edge->vertex[1]->v.v;
 
142
    edge->dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &edge->witness);
 
143
 
 
144
    ccdListAppend(&edge->vertex[0]->edges, &edge->vertex_list[0]);
 
145
    ccdListAppend(&edge->vertex[1]->edges, &edge->vertex_list[1]);
 
146
 
 
147
    ccdListAppend(&pt->edges, &edge->list);
 
148
 
 
149
    // update position in .nearest array
 
150
    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)edge);
 
151
 
 
152
    return edge;
 
153
}
 
154
 
 
155
ccd_pt_face_t *ccdPtAddFace(ccd_pt_t *pt, ccd_pt_edge_t *e1,
 
156
                                          ccd_pt_edge_t *e2,
 
157
                                          ccd_pt_edge_t *e3)
 
158
{
 
159
    const ccd_vec3_t *a, *b, *c;
 
160
    ccd_pt_face_t *face;
 
161
    ccd_pt_edge_t *e;
 
162
    size_t i;
 
163
 
 
164
    if (e1 == NULL || e2 == NULL || e3 == NULL)
 
165
        return NULL;
 
166
 
 
167
    face = CCD_ALLOC(ccd_pt_face_t);
 
168
    if (face == NULL)
 
169
        return NULL;
 
170
 
 
171
    face->type = CCD_PT_FACE;
 
172
    face->edge[0] = e1;
 
173
    face->edge[1] = e2;
 
174
    face->edge[2] = e3;
 
175
 
 
176
    // obtain triplet of vertices
 
177
    a = &face->edge[0]->vertex[0]->v.v;
 
178
    b = &face->edge[0]->vertex[1]->v.v;
 
179
    e = face->edge[1];
 
180
    if (e->vertex[0] != face->edge[0]->vertex[0]
 
181
            && e->vertex[0] != face->edge[0]->vertex[1]){
 
182
        c = &e->vertex[0]->v.v;
 
183
    }else{
 
184
        c = &e->vertex[1]->v.v;
 
185
    }
 
186
    face->dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &face->witness);
 
187
 
 
188
 
 
189
    for (i = 0; i < 3; i++){
 
190
        if (face->edge[i]->faces[0] == NULL){
 
191
            face->edge[i]->faces[0] = face;
 
192
        }else{
 
193
            face->edge[i]->faces[1] = face;
 
194
        }
 
195
    }
 
196
 
 
197
    ccdListAppend(&pt->faces, &face->list);
 
198
 
 
199
    // update position in .nearest array
 
200
    _ccdPtNearestUpdate(pt, (ccd_pt_el_t *)face);
 
201
 
 
202
    return face;
 
203
}
 
204
 
 
205
 
 
206
void ccdPtRecomputeDistances(ccd_pt_t *pt)
 
207
{
 
208
    ccd_pt_vertex_t *v;
 
209
    ccd_pt_edge_t *e;
 
210
    ccd_pt_face_t *f;
 
211
    const ccd_vec3_t *a, *b, *c;
 
212
    ccd_real_t dist;
 
213
 
 
214
    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
 
215
        dist = ccdVec3Len2(&v->v.v);
 
216
        v->dist = dist;
 
217
        ccdVec3Copy(&v->witness, &v->v.v);
 
218
    }
 
219
 
 
220
    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
 
221
        a = &e->vertex[0]->v.v;
 
222
        b = &e->vertex[1]->v.v;
 
223
        dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, a, b, &e->witness);
 
224
        e->dist = dist;
 
225
    }
 
226
 
 
227
    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
 
228
        // obtain triplet of vertices
 
229
        a = &f->edge[0]->vertex[0]->v.v;
 
230
        b = &f->edge[0]->vertex[1]->v.v;
 
231
        e = f->edge[1];
 
232
        if (e->vertex[0] != f->edge[0]->vertex[0]
 
233
                && e->vertex[0] != f->edge[0]->vertex[1]){
 
234
            c = &e->vertex[0]->v.v;
 
235
        }else{
 
236
            c = &e->vertex[1]->v.v;
 
237
        }
 
238
 
 
239
        dist = ccdVec3PointTriDist2(ccd_vec3_origin, a, b, c, &f->witness);
 
240
        f->dist = dist;
 
241
    }
 
242
}
 
243
 
 
244
ccd_pt_el_t *ccdPtNearest(ccd_pt_t *pt)
 
245
{
 
246
    if (!pt->nearest){
 
247
        _ccdPtNearestRenew(pt);
 
248
    }
 
249
    return pt->nearest;
 
250
}
 
251
 
 
252
 
 
253
void ccdPtDumpSVT(ccd_pt_t *pt, const char *fn)
 
254
{
 
255
    FILE *fout;
 
256
 
 
257
    fout = fopen(fn, "a");
 
258
    if (fout == NULL)
 
259
        return;
 
260
 
 
261
    ccdPtDumpSVT2(pt, fout);
 
262
 
 
263
    fclose(fout);
 
264
}
 
265
 
 
266
void ccdPtDumpSVT2(ccd_pt_t *pt, FILE *fout)
 
267
{
 
268
    ccd_pt_vertex_t *v, *a, *b, *c;
 
269
    ccd_pt_edge_t *e;
 
270
    ccd_pt_face_t *f;
 
271
    size_t i;
 
272
 
 
273
    fprintf(fout, "-----\n");
 
274
 
 
275
    fprintf(fout, "Points:\n");
 
276
    i = 0;
 
277
    ccdListForEachEntry(&pt->vertices, v, ccd_pt_vertex_t, list){
 
278
        v->id = i++;
 
279
        fprintf(fout, "%lf %lf %lf\n",
 
280
                ccdVec3X(&v->v.v), ccdVec3Y(&v->v.v), ccdVec3Z(&v->v.v));
 
281
    }
 
282
 
 
283
    fprintf(fout, "Edges:\n");
 
284
    ccdListForEachEntry(&pt->edges, e, ccd_pt_edge_t, list){
 
285
        fprintf(fout, "%d %d\n", e->vertex[0]->id, e->vertex[1]->id);
 
286
    }
 
287
 
 
288
    fprintf(fout, "Faces:\n");
 
289
    ccdListForEachEntry(&pt->faces, f, ccd_pt_face_t, list){
 
290
        a = f->edge[0]->vertex[0];
 
291
        b = f->edge[0]->vertex[1];
 
292
        c = f->edge[1]->vertex[0];
 
293
        if (c == a || c == b){
 
294
            c = f->edge[1]->vertex[1];
 
295
        }
 
296
        fprintf(fout, "%d %d %d\n", a->id, b->id, c->id);
 
297
    }
 
298
}