coeng: 8cce82794f90 src/geom.cc

coeng

view src/geom.cc @ 8:8cce82794f90

seems to work nicely

author	John Tsiombikas <nuclear@member.fsf.org>
date	Sun, 15 Feb 2015 05:14:20 +0200
parents	af24cfbdf9b6
children

line source

1 #include "opengl.h"

2 #include <float.h>

3 #include <algorithm>

4 #include "geom.h"

6 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit);

7 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit);

8 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit);

10 GeomShape::GeomShape()

11 {

12 type = GEOM_UNKNOWN;

13 }

15 GeomShape::~GeomShape()

16 {

17 }

19 GeomShape::Type GeomShape::get_type() const

20 {

21 return type;

22 }

24 void GeomShape::set_transform(const Matrix4x4 &m)

25 {

26 xform = m;

27 inv_xform = xform.inverse();

28 }

30 const Matrix4x4 &GeomShape::get_transform() const

31 {

32 return xform;

33 }

35 const Matrix4x4 &GeomShape::get_inv_transform() const

36 {

37 return inv_xform;

38 }

40 // ---- Sphere class ----

42 Sphere::Sphere()

43 {

44 type = GEOM_SPHERE;

45 radius = 1.0f;

46 }

48 Sphere::Sphere(const Vector3 &c, float rad)

49 : center(c)

50 {

51 type = GEOM_SPHERE;

52 radius = rad;

53 }

55 bool Sphere::contains(const Vector3 &pt) const

56 {

57 return distance_sq(pt) <= 0.0;

58 }

60 bool Sphere::intersect(const Ray &inray, HitPoint *hit) const

61 {

62 Ray ray = inray.transformed(inv_xform);

64 float a = dot_product(ray.dir, ray.dir);

65 float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +

66 2.0 * ray.dir.y * (ray.origin.y - center.y) +

67 2.0 * ray.dir.z * (ray.origin.z - center.z);

68 float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) -

69 2.0 * dot_product(ray.origin, center) - radius * radius;

71 float discr = b * b - 4.0 * a * c;

72 if(discr < 1e-4) {

73 return false;

74 }

76 float sqrt_discr = sqrt(discr);

77 float t0 = (-b + sqrt_discr) / (2.0 * a);

78 float t1 = (-b - sqrt_discr) / (2.0 * a);

80 if(t0 < 1e-4)

81 t0 = t1;

82 if(t1 < 1e-4)

83 t1 = t0;

85 float t = t0 < t1 ? t0 : t1;

86 if(t < 1e-4) {

87 return false;

88 }

90 // fill the HitPoint structure

91 if(hit) {

92 hit->shape = this;

93 hit->data = 0;

94 hit->dist = t;

96 Vector3 local_pos = ray.origin + ray.dir * t;

97 hit->pos = inray.origin + inray.dir * t;

98 hit->normal = (local_pos - center) / radius;

99 }

100 return true;

101 }

102

103 bool Sphere::collide(const GeomShape *geom, HitPoint *hit) const

104 {

105 switch(geom->get_type()) {

106 case GEOM_SPHERE:

107 return col_sphere_sphere(this, (const Sphere*)geom, hit);

108 case GEOM_PLANE:

109 return col_sphere_plane(this, (const Plane*)geom, hit);

110 default:

111 break;

112 }

113 return false;

114 }

115

116 float Sphere::distance(const Vector3 &pt) const

117 {

118 Vector3 local_pt = pt.transformed(inv_xform);

119 return (local_pt - center).length() - radius;

120 }

121

122 float Sphere::distance_sq(const Vector3 &pt) const

123 {

124 Vector3 local_pt = pt.transformed(inv_xform);

125 return (local_pt - center).length_sq() - radius * radius;

126 }

127

128 void Sphere::draw() const

129 {

130 // TODO

131 }

132

133 // ---- Plane class ----

134

135 Plane::Plane()

136 : normal(0, 1, 0)

137 {

138 type = GEOM_PLANE;

139 dist = 0.0f;

140 }

141

142 Plane::Plane(const Vector3 &n, float d)

143 : normal(n)

144 {

145 type = GEOM_PLANE;

146 dist = d;

147

148 normal.normalize();

149 }

150

151 Plane::Plane(float a, float b, float c, float d)

152 : normal(a, b, c)

153 {

154 type = GEOM_PLANE;

155 dist = d;

156

157 normal.normalize();

158 }

159

160 Plane::Plane(const Vector3 &pos, const Vector3 &norm)

161 : normal(norm)

162 {

163 type = GEOM_PLANE;

164

165 dist = 0.0f;

166 dist = distance(pos);

167 }

168

169 Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3)

170 {

171 type = GEOM_PLANE;

172

173 normal = cross_product(p2 - p1, p3 - p1).normalized();

174 dist = 0.0f;

175 dist = distance(p1);

176 }

177

178 bool Plane::contains(const Vector3 &pt) const

179 {

180 return distance(pt) <= 0.0;

181 }

182

183 bool Plane::intersect(const Ray &inray, HitPoint *hit) const

184 {

185 Ray ray = inray.transformed(inv_xform);

186 Vector3 pt = normal * dist;

187

188 float ndotdir = dot_product(normal, ray.dir);

189 if(fabs(ndotdir) < 1e-4) {

190 return false;

191 }

192

193 if(hit) {

194 Vector3 ptdir = pt - ray.origin;

195 float t = dot_product(normal, ptdir) / ndotdir;

196

197 hit->pos = inray.origin + inray.dir * t;

198 hit->normal = normal;

199 hit->shape = this;

200 }

201 return true;

202 }

203

204 bool Plane::collide(const GeomShape *geom, HitPoint *hit) const

205 {

206 switch(geom->get_type()) {

207 case GEOM_SPHERE:

208 {

209 bool res = col_sphere_plane((const Sphere*)geom, this, hit);

210 if(hit) {

211 hit->normal = -hit->normal;

212 }

213 return res;

214 }

215

216 case GEOM_PLANE:

217 return col_plane_plane(this, (const Plane*)geom, hit);

218

219 default:

220 break;

221 }

222 return false;

223 }

224

225 float Plane::distance(const Vector3 &v) const

226 {

227 Vector3 pt = normal * dist;

228 return dot_product(v.transformed(inv_xform) - pt, normal);

229 }

230

231 float Plane::distance_sq(const Vector3 &v) const

232 {

233 float d = distance(v);

234 return d * d;

235 }

236

237 void Plane::draw() const

238 {

239 // TODO

240 }

241

242 // ---- AABox ----

243

244 AABox::AABox()

245 {

246 type = GEOM_AABOX;

247 }

248

249 AABox::AABox(const Vector3 &vmin, const Vector3 &vmax)

250 : min(vmin), max(vmax)

251 {

252 type = GEOM_AABOX;

253 }

254

255 void AABox::set_union(const GeomShape *obj1, const GeomShape *obj2)

256 {

257 const AABox *box1 = (const AABox*)obj1;

258 const AABox *box2 = (const AABox*)obj2;

259

260 if(!box1 || !box2 || obj1->get_type() != GEOM_AABOX || obj2->get_type() != GEOM_AABOX) {

261 fprintf(stderr, "AABox::set_union: arguments must be AABoxes too\n");

262 return;

263 }

264

265 min.x = std::min(box1->min.x, box2->min.x);

266 min.y = std::min(box1->min.y, box2->min.y);

267 min.z = std::min(box1->min.z, box2->min.z);

268

269 max.x = std::max(box1->max.x, box2->max.x);

270 max.y = std::max(box1->max.y, box2->max.y);

271 max.z = std::max(box1->max.z, box2->max.z);

272 }

273

274 void AABox::set_intersection(const GeomShape *obj1, const GeomShape *obj2)

275 {

276 const AABox *box1 = (const AABox*)obj1;

277 const AABox *box2 = (const AABox*)obj2;

278

279 if(!box1 || !box2 || obj1->get_type() != GEOM_AABOX || obj2->get_type() != GEOM_AABOX) {

280 fprintf(stderr, "AABox::set_union: arguments must be AABoxes too\n");

281 return;

282 }

283

284 for(int i=0; i<3; i++) {

285 min[i] = std::max(box1->min[i], box2->min[i]);

286 max[i] = std::min(box1->max[i], box2->max[i]);

287

288 if(max[i] < min[i]) {

289 max[i] = min[i];

290 }

291 }

292 }

293

294 bool AABox::contains(const Vector3 &pt) const

295 {

296 return pt.x >= min.x && pt.x <= max.x &&

297 pt.y >= min.y && pt.y <= max.y &&

298 pt.z >= min.z && pt.z <= max.z;

299 }

300

301 bool AABox::intersect(const Ray &inray, HitPoint *hit) const

302 {

303 Ray ray = inray.transformed(inv_xform);

304

305 Vector3 param[2] = {min, max};

306 Vector3 inv_dir(1.0 / ray.dir.x, 1.0 / ray.dir.y, 1.0 / ray.dir.z);

307 int sign[3] = {inv_dir.x < 0, inv_dir.y < 0, inv_dir.z < 0};

308

309 float tmin = (param[sign[0]].x - ray.origin.x) * inv_dir.x;

310 float tmax = (param[1 - sign[0]].x - ray.origin.x) * inv_dir.x;

311 float tymin = (param[sign[1]].y - ray.origin.y) * inv_dir.y;

312 float tymax = (param[1 - sign[1]].y - ray.origin.y) * inv_dir.y;

313

314 if(tmin > tymax || tymin > tmax) {

315 return false;

316 }

317 if(tymin > tmin) {

318 tmin = tymin;

319 }

320 if(tymax < tmax) {

321 tmax = tymax;

322 }

323

324 float tzmin = (param[sign[2]].z - ray.origin.z) * inv_dir.z;

325 float tzmax = (param[1 - sign[2]].z - ray.origin.z) * inv_dir.z;

326

327 if(tmin > tzmax || tzmin > tmax) {

328 return false;

329 }

330 if(tzmin > tmin) {

331 tmin = tzmin;

332 }

333 if(tzmax < tmax) {

334 tmax = tzmax;

335 }

336

337 float t = tmin < 1e-4 ? tmax : tmin;

338 if(t >= 1e-4) {

339

340 if(hit) {

341 hit->shape = this;

342 hit->dist = t;

343 hit->pos = inray.origin + inray.dir * t;

344

345 float min_dist = FLT_MAX;

346 Vector3 offs = min + (max - min) / 2.0;

347 Vector3 local_hit = hit->pos - offs;

348

349 static const Vector3 axis[] = {

350 Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)

351 };

352 //int tcidx[][2] = {{2, 1}, {0, 2}, {0, 1}};

353

354 for(int i=0; i<3; i++) {

355 float dist = fabs((max[i] - offs[i]) - fabs(local_hit[i]));

356 if(dist < min_dist) {

357 min_dist = dist;

358 hit->normal = axis[i] * (local_hit[i] < 0.0 ? 1.0 : -1.0);

359 //hit->texcoord = Vector2(hit->pos[tcidx[i][0]], hit->pos[tcidx[i][1]]);

360 }

361 }

362 hit->normal.transform(Matrix3x3(xform));

363 }

364 return true;

365 }

366 return false;

367 }

368

369 bool AABox::collide(const GeomShape *geom, HitPoint *hit) const

370 {

371 fprintf(stderr, "%s: not implemented yet\n", __FUNCTION__);

372 return false;

373 }

374

375 float AABox::distance(const Vector3 &pt) const

376 {

377 float d = distance_sq(pt);

378 return d * d;

379 }

380

381 float AABox::distance_sq(const Vector3 &world_pt) const

382 {

383 Vector3 pt = pt.transformed(inv_xform);

384

385 float dx = std::max(pt.x - max.x, min.x - pt.x);

386 float dy = std::max(pt.y - max.y, min.y - pt.y);

387 float dz = std::max(pt.z - max.z, min.z - pt.z);

388

389 return Vector3(dx, dy, dz).length_sq();

390 }

391

392 void AABox::draw() const

393 {

394 }

395

396

397 // collision detection functions

398

399 static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit)

400 {

401 const Matrix4x4 &xforma = a->get_transform();

402 const Matrix4x4 &xformb = b->get_transform();

403

404 Vector3 ca = a->center.transformed(xforma);

405 Vector3 cb = b->center.transformed(xformb);

406 float rada = a->radius * xforma.get_scaling().x;

407 float radb = b->radius * xformb.get_scaling().x;

408

409 float sum_rad = rada + radb;

410 Vector3 dir = cb - ca;

411 float dist_sq = dir.length_sq();

412 bool res = dist_sq <= sum_rad * sum_rad;

413

414 if(res && hit) {

415 hit->pos = ca + dir * 0.5;

416 hit->shape = b;

417 hit->normal = -dir.normalized();

418 }

419 return res;

420 }

421

422 static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit)

423 {

424 fprintf(stderr, "%s: not implemented\n", __FUNCTION__);

425 return false;

426 }

427

428 static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit)

429 {

430 const Matrix4x4 &xform = sph->get_transform();

431

432 Vector3 sph_center = sph->center.transformed(xform);

433 float sph_rad = sph->radius * xform.get_scaling().x;

434

435 float dist = plane->distance(sph_center);

436 bool res = fabs(dist) <= sph_rad;

437

438 if(res && hit) {

439 Vector3 planept = (plane->normal * plane->dist).transformed(plane->get_transform());

440 Vector3 planenorm = plane->normal.transformed(Matrix3x3(plane->get_transform()));

441

442 Vector3 sphdir = sph_center - planept;

443 if(dot_product(sphdir, planenorm) >= 0.0f) {

444 hit->normal = planenorm;

445 } else {

446 hit->normal = -planenorm;

447 }

448 hit->pos = sph_center - planenorm * fabs(dist);

449 hit->shape = plane;

450 }

451 return res;

452 }