blob: 97a5834f342e44689e9e7671501d13fdf135ae1f (
plain)
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
|
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "GeomPerfectMatching.h"
#include "GPMkdtree.h"
GeomPerfectMatching::GeomPerfectMatching(int nodeNum, int _DIM)
: DIM(_DIM),
node_num(0),
node_num_max(nodeNum),
edge_num(0)
{
if (node_num_max < 1) { printf("too few nodes\n"); exit(1); }
if (node_num_max & 1) { printf("# of points is odd: perfect matching cannot exist\n"); exit(1); }
nodes = (Node*) malloc(node_num_max*sizeof(Node));
memset(nodes, 0, node_num_max*sizeof(Node));
edges = new Block<Edge>(512);
coords = (REAL*) malloc((DIM+1)*node_num_max*sizeof(REAL));
sums = coords + DIM*node_num_max;
matching = (int*) malloc(node_num_max*sizeof(int));
int i;
for (i=0; i<node_num_max; i++) matching[i] = i;
}
GeomPerfectMatching::~GeomPerfectMatching()
{
free(nodes);
delete edges;
free(coords);
free(matching);
}
GeomPerfectMatching::PointId GeomPerfectMatching::AddPoint(REAL* coord)
{
if (node_num >= node_num_max)
{
printf("Error: you are trying to add too many points!\n");
exit(1);
}
memcpy(coords+DIM*node_num, coord, DIM*sizeof(REAL));
return node_num ++;
}
void GeomPerfectMatching::AddInitialEdge(PointId _i, PointId _j)
{
assert(_i>=0 && _i<node_num_max && _j>=0 && _j<node_num_max && _i!=_j);
if (_j < _i) { int _k = _i; _i = _j; _j = _k; }
Node* i = nodes + _i;
Node* j = nodes + _j;
Edge* e = edges->New();
edge_num ++;
e->head[1] = _i;
e->head[0] = _j;
e->next[0] = i->first[0];
e->next[1] = j->first[1];
i->first[0] = e;
j->first[1] = e;
}
GeomPerfectMatching::REAL GeomPerfectMatching::ComputeCost(PointId* matching)
{
if (node_num != node_num_max) { printf("ComputeCost() cannot be called before all points have been added!\n"); exit(1); }
REAL cost = 0;
int i;
for (i=0; i<node_num; i++)
{
if (matching[i]==i || matching[i]<0 || matching[i]>=node_num || matching[matching[i]]!=i)
{
printf("ComputeCost(): not a valid matching!\n");
exit(1);
}
if (matching[i] > i)
{
cost += Dist(i, matching[i]);
}
}
return cost;
}
|
