libccd/src/testsuites/vec3.c

   1 #include <stdio.h>
   2 #include <cu/cu.h>
   3 #include <ccd/vec3.h>
   4
   5 TEST(vec3SetUp)
   6 {
   7 }
   8
   9 TEST(vec3TearDown)
  10 {
  11 }
  12
  13
  14 TEST(vec3PointSegmentDist)
  15 {
  16     ccd_vec3_t P, a, b, w, ew;
  17     ccd_real_t dist;
  18
  19     ccdVec3Set(&a, 0., 0., 0.);
  20     ccdVec3Set(&b, 1., 0., 0.);
  21
  22     // extreme w == a
  23     ccdVec3Set(&P, -1., 0., 0.);
  24     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  25     assertTrue(ccdEq(dist, 1.));
  26     assertTrue(ccdVec3Eq(&w, &a));
  27
  28     ccdVec3Set(&P, -0.5, 0., 0.);
  29     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  30     assertTrue(ccdEq(dist, 0.5 * 0.5));
  31     assertTrue(ccdVec3Eq(&w, &a));
  32
  33     ccdVec3Set(&P, -0.1, 0., 0.);
  34     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  35     assertTrue(ccdEq(dist, .1 * .1));
  36     assertTrue(ccdVec3Eq(&w, &a));
  37
  38     ccdVec3Set(&P, 0., 0., 0.);
  39     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  40     assertTrue(ccdEq(dist, 0.));
  41     assertTrue(ccdVec3Eq(&w, &a));
  42
  43     ccdVec3Set(&P, -1., 1., 0.);
  44     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  45     assertTrue(ccdEq(dist, 2.));
  46     assertTrue(ccdVec3Eq(&w, &a));
  47
  48     ccdVec3Set(&P, -0.5, 0.5, 0.);
  49     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  50     assertTrue(ccdEq(dist, 0.5));
  51     assertTrue(ccdVec3Eq(&w, &a));
  52
  53     ccdVec3Set(&P, -0.1, -1., 2.);
  54     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  55     assertTrue(ccdEq(dist, 5.01));
  56     assertTrue(ccdVec3Eq(&w, &a));
  57
  58
  59     // extreme w == b
  60     ccdVec3Set(&P, 2., 0., 0.);
  61     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  62     assertTrue(ccdEq(dist, 1.));
  63     assertTrue(ccdVec3Eq(&w, &b));
  64
  65     ccdVec3Set(&P, 1.5, 0., 0.);
  66     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  67     assertTrue(ccdEq(dist, 0.5 * 0.5));
  68     assertTrue(ccdVec3Eq(&w, &b));
  69
  70     ccdVec3Set(&P, 1.1, 0., 0.);
  71     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  72     assertTrue(ccdEq(dist, .1 * .1));
  73     assertTrue(ccdVec3Eq(&w, &b));
  74
  75     ccdVec3Set(&P, 1., 0., 0.);
  76     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  77     assertTrue(ccdEq(dist, 0.));
  78     assertTrue(ccdVec3Eq(&w, &b));
  79
  80     ccdVec3Set(&P, 2., 1., 0.);
  81     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  82     assertTrue(ccdEq(dist, 2.));
  83     assertTrue(ccdVec3Eq(&w, &b));
  84
  85     ccdVec3Set(&P, 1.5, 0.5, 0.);
  86     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  87     assertTrue(ccdEq(dist, 0.5));
  88     assertTrue(ccdVec3Eq(&w, &b));
  89
  90     ccdVec3Set(&P, 1.1, -1., 2.);
  91     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  92     assertTrue(ccdEq(dist, 5.01));
  93     assertTrue(ccdVec3Eq(&w, &b));
  94
  95     // inside segment
  96     ccdVec3Set(&P, .5, 0., 0.);
  97     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
  98     assertTrue(ccdEq(dist, 0.));
  99     assertTrue(ccdVec3Eq(&w, &P));
 100
 101     ccdVec3Set(&P, .9, 0., 0.);
 102     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 103     assertTrue(ccdEq(dist, 0.));
 104     assertTrue(ccdVec3Eq(&w, &P));
 105
 106     ccdVec3Set(&P, .5, 1., 0.);
 107     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 108     assertTrue(ccdEq(dist, 1.));
 109     ccdVec3Set(&ew, 0.5, 0., 0.);
 110     assertTrue(ccdVec3Eq(&w, &ew));
 111
 112     ccdVec3Set(&P, .5, 1., 1.);
 113     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 114     assertTrue(ccdEq(dist, 2.));
 115     ccdVec3Set(&ew, 0.5, 0., 0.);
 116     assertTrue(ccdVec3Eq(&w, &ew));
 117
 118
 119
 120     ccdVec3Set(&a, -.5, 2., 1.);
 121     ccdVec3Set(&b, 1., 1.5, 0.5);
 122
 123     // extreme w == a
 124     ccdVec3Set(&P, -10., 0., 0.);
 125     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 126     assertTrue(ccdEq(dist, 9.5 * 9.5 + 2. * 2. + 1.));
 127     assertTrue(ccdVec3Eq(&w, &a));
 128
 129     ccdVec3Set(&P, -10., 9.2, 3.4);
 130     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 131     assertTrue(ccdEq(dist, 9.5 * 9.5 + 7.2 * 7.2 + 2.4 * 2.4));
 132     assertTrue(ccdVec3Eq(&w, &a));
 133
 134     // extreme w == b
 135     ccdVec3Set(&P, 10., 0., 0.);
 136     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 137     assertTrue(ccdEq(dist, 9. * 9. + 1.5 * 1.5 + 0.5 * 0.5));
 138     assertTrue(ccdVec3Eq(&w, &b));
 139
 140     ccdVec3Set(&P, 10., 9.2, 3.4);
 141     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 142     assertTrue(ccdEq(dist, 9. * 9. + 7.7 * 7.7 + 2.9 * 2.9));
 143     assertTrue(ccdVec3Eq(&w, &b));
 144
 145     // inside ab
 146     ccdVec3Set(&a, -.1, 1., 1.);
 147     ccdVec3Set(&b, 1., 1., 1.);
 148     ccdVec3Set(&P, 0., 0., 0.);
 149     dist = ccdVec3PointSegmentDist2(&P, &a, &b, &w);
 150     assertTrue(ccdEq(dist, 2.));
 151     ccdVec3Set(&ew, 0., 1., 1.);
 152     assertTrue(ccdVec3Eq(&w, &ew));
 153 }
 154
 155
 156 TEST(vec3PointTriDist)
 157 {
 158     ccd_vec3_t P, a, b, c, w, P0;
 159     ccd_real_t dist;
 160
 161     ccdVec3Set(&a, -1., 0., 0.);
 162     ccdVec3Set(&b, 0., 1., 1.);
 163     ccdVec3Set(&c, -1., 0., 1.);
 164
 165     ccdVec3Set(&P, -1., 0., 0.);
 166     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 167     assertTrue(ccdEq(dist, 0.));
 168     assertTrue(ccdVec3Eq(&w, &a));
 169
 170     ccdVec3Set(&P, 0., 1., 1.);
 171     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 172     assertTrue(ccdEq(dist, 0.));
 173     assertTrue(ccdVec3Eq(&w, &b));
 174
 175     ccdVec3Set(&P, -1., 0., 1.);
 176     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 177     assertTrue(ccdEq(dist, 0.));
 178     assertTrue(ccdVec3Eq(&w, &c));
 179
 180     ccdVec3Set(&P, 0., 0., 0.);
 181     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, NULL);
 182     assertTrue(ccdEq(dist, 2./3.));
 183
 184
 185     // region 4
 186     ccdVec3Set(&P, -2., 0., 0.);
 187     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 188     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a)));
 189     assertTrue(ccdVec3Eq(&w, &a));
 190     ccdVec3Set(&P, -2., 0.2, -1.);
 191     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 192     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &a)));
 193     assertTrue(ccdVec3Eq(&w, &a));
 194
 195     // region 2
 196     ccdVec3Set(&P, -1.3, 0., 1.2);
 197     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 198     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c)));
 199     assertTrue(ccdVec3Eq(&w, &c));
 200     ccdVec3Set(&P, -1.2, 0.2, 1.1);
 201     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 202     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &c)));
 203     assertTrue(ccdVec3Eq(&w, &c));
 204
 205     // region 6
 206     ccdVec3Set(&P, 0.3, 1., 1.);
 207     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 208     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b)));
 209     assertTrue(ccdVec3Eq(&w, &b));
 210     ccdVec3Set(&P, .1, 1., 1.);
 211     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 212     assertTrue(ccdEq(dist, ccdVec3Dist2(&P, &b)));
 213     assertTrue(ccdVec3Eq(&w, &b));
 214
 215     // region 1
 216     ccdVec3Set(&P, 0., 1., 2.);
 217     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 218     assertTrue(ccdEq(dist, 1.));
 219     assertTrue(ccdVec3Eq(&w, &b));
 220     ccdVec3Set(&P, -1., 0., 2.);
 221     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 222     assertTrue(ccdEq(dist, 1.));
 223     assertTrue(ccdVec3Eq(&w, &c));
 224     ccdVec3Set(&P, -0.5, 0.5, 2.);
 225     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 226     assertTrue(ccdEq(dist, 1.));
 227     ccdVec3Set(&P0, -0.5, 0.5, 1.);
 228     assertTrue(ccdVec3Eq(&w, &P0));
 229
 230     // region 3
 231     ccdVec3Set(&P, -2., -1., 0.7);
 232     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 233     assertTrue(ccdEq(dist, 2.));
 234     ccdVec3Set(&P0, -1., 0., 0.7);
 235     assertTrue(ccdVec3Eq(&w, &P0));
 236
 237     // region 5
 238     ccdVec3Set(&P, 0., 0., 0.);
 239     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 240     assertTrue(ccdEq(dist, 2./3.));
 241     ccdVec3Set(&P0, -2./3., 1./3., 1./3.);
 242     assertTrue(ccdVec3Eq(&w, &P0));
 243
 244     // region 0
 245     ccdVec3Set(&P, -0.5, 0.5, 0.5);
 246     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 247     assertTrue(ccdEq(dist, 0.));
 248     assertTrue(ccdVec3Eq(&w, &P));
 249     ccdVec3Set(&P, -0.5, 0.5, 0.7);
 250     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 251     assertTrue(ccdEq(dist, 0.));
 252     assertTrue(ccdVec3Eq(&w, &P));
 253     ccdVec3Set(&P, -0.5, 0.5, 0.9);
 254     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 255     assertTrue(ccdEq(dist, 0.));
 256     assertTrue(ccdVec3Eq(&w, &P));
 257
 258     ccdVec3Set(&P, 0., 0., 0.5);
 259     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 260     assertTrue(ccdEq(dist, 0.5));
 261     ccdVec3Set(&P0, -.5, .5, .5);
 262     assertTrue(ccdVec3Eq(&w, &P0));
 263
 264     ccdVec3Set(&a, -1., 0., 0.);
 265     ccdVec3Set(&b, 0., 1., -1.);
 266     ccdVec3Set(&c, 0., 1., 1.);
 267     ccdVec3Set(&P, 0., 0., 0.);
 268     dist = ccdVec3PointTriDist2(&P, &a, &b, &c, &w);
 269     assertTrue(ccdEq(dist, 0.5));
 270     ccdVec3Set(&P0, -.5, .5, 0.);
 271     assertTrue(ccdVec3Eq(&w, &P0));
 272     //fprintf(stderr, "dist: %lf\n", dist);
 273 }