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
|
#include "../../../include/odfaeg/Physics/boundingSphere.h"
#include "../../../include/odfaeg/Physics/boundingEllipsoid.h"
#include "../../../include/odfaeg/Physics/boundingBox.h"
#include "../../../include/odfaeg/Physics/orientedBoundingBox.h"
#include "../../../include/odfaeg/Physics/boundingPolyhedron.h"
#include "../../../include/odfaeg/Graphics/transformMatrix.h"
using namespace std;
namespace odfaeg {
namespace physic {
/*Crée un cercle englobant, utilisé pour les détection de collisions, et la sélection
* d'objets.
*/
//Crée un cercle de centre center et de rayon radius.
BoundingSphere::BoundingSphere() {
center = math::Vec3f(0, 0, 0);
radius = 1;
}
BoundingSphere::BoundingSphere (math::Vec3f center, float radius) {
this->center = center;
this->radius = radius;
}
bool BoundingSphere::intersects (BoundingSphere &other, CollisionResultSet::Info& info) {
float d = center.computeDist(other.center);
float rSum = radius + other.radius;
return (d <= rSum);
}
bool BoundingSphere::intersects (BoundingEllipsoid &be, CollisionResultSet::Info& info) {
return be.intersects(*this, info);
}
bool BoundingSphere::intersects (BoundingBox &bx, CollisionResultSet::Info& info) {
float dist;
int pIndex, eIndex, fIndex;
std::vector<math::Vec3f> point = {center};
int vIndex = math::Computer::checkNearestVertexFromShape(bx.getCenter(), bx.getVertices(), bx.getEdgeBissectors(), bx.getEdgeNormals(),
bx.getFaceBissectors(), bx.getFaceNormals(),point,dist,pIndex,eIndex,fIndex,4);
info.nearestVertexIndex1 = vIndex;
info.nearestPtIndex1 = pIndex;
info.nearestEdgeIndex1 = eIndex;
info.nearestFaceIndex1 = fIndex;
math::Vec3f v = center - bx.getCenter();
if (pIndex != -1)
return v.magnitude() <= radius + (bx.getVertices()[pIndex] - bx.getCenter()).magnitude();
math::Vec3f n = bx.getEdgeNormals()[eIndex];
float pn = math::Math::abs(v.projOnAxis(n));
info.mtu = n * (radius + (bx.getEdgeBissectors()[eIndex] - bx.getCenter()).magnitude() - pn);
return pn <= radius + (bx.getEdgeBissectors()[eIndex] - bx.getCenter()).magnitude();
}
bool BoundingSphere::intersects (OrientedBoundingBox &obx, CollisionResultSet::Info& info) {
math::Ray ray(obx.getCenter(), center);
math::Vec3f near, far;
if (!obx.intersectsWhere(ray, near, far, info)) {
return false;
}
math::Vec3f d = far - obx.getCenter();
return (center.computeDistSquared(obx.getCenter()) - radius * radius - d.magnSquared()) <= 0;
}
bool BoundingSphere::intersects (BoundingPolyhedron &bp, CollisionResultSet::Info& info) {
math::Ray ray(bp.getCenter(), center);
math::Vec3f near, far;
if (!bp.intersectsWhere(ray, near, far, info)) {
return false;
}
math::Vec3f d = far - bp.getCenter();
return (center.computeDistSquared(bp.getCenter()) - radius * radius - d.magnSquared()) <= 0;
}
bool BoundingSphere::intersects (math::Ray &ray, bool segment, CollisionResultSet::Info& info) {
if (isPointInside(ray.getOrig()))
return true;
math::Vec3f d1 = center - ray.getOrig();
float dp = d1.dot2(ray.getDir());
if (dp < 0)
return false;
math::Vec3f p = ray.getOrig() + ray.getDir().normalize() * d1.projOnAxis(ray.getDir());
math::Vec3f d2 = p - center;
if (!segment) {
return d2.magnSquared() <= radius * radius;
} else {
if(d2.magnSquared() > radius * radius) {
return false;
} else {
math::Vec3f near, far;
intersectsWhere(ray, near, far, info);
math::Vec3f v1 = near - ray.getOrig();
math::Vec3f v2 = far - ray.getOrig();
math::Vec3f d = ray.getExt() - ray.getOrig();
float i1 = v1.magnSquared() / d.magnSquared();
float i2 = v2.magnSquared() / d.magnSquared();
if (i1 <= 1 || i2 <= 1)
return true;
return false;
}
}
}
bool BoundingSphere::intersectsWhere (math::Ray& r, math::Vec3f& near, math::Vec3f &far, CollisionResultSet::Info& info) {
math::Vec3f p = r.getOrig();
math::Vec3f d = r.getDir().normalize();
math::Vec3f c = center;
// this is the vector from p to c
math::Vec3f vpc = c - p;
if (vpc.dot2(d) < 0) {
// when the sphere is behind the origin p
// note that this case may be dismissed if it is
// considered that p is outside the sphere
if (vpc.magnSquared() > radius * radius) {
return false; // there is no intersection
} else if (vpc.magnSquared() == radius * radius) {
near = far = p;
return true;
} else {
// occurs when p is inside the sphere
math::Vec3f pc = p + d * vpc.projOnAxis(d);
float dist = math::Math::sqrt(radius * radius - pc.computeDistSquared(c));
float di1 = dist - pc.computeDist(p);
near = p;
far = p + d * di1;
return true;
}
} else {
// center of sphere projects on the ray
math::Vec3f pc = p + d * vpc.projOnAxis(d);
if (c.computeDistSquared(pc) > radius * radius) {
// there is no intersection
return false;
} else {
// distance from pc to i1 and to i2.
float dist = math::Math::sqrt((radius * radius) - pc.computeDistSquared(c));
float di1, di2;
if(vpc.magnSquared() > radius * radius) {
di1 = pc.computeDist(p) - dist;
near = p + d * di1;
di2 = pc.computeDist(p) + dist;
far = p + d * di2;
} else {
// origin is inside the sphere
di1 = pc.computeDist(p) + dist;
near = p;
far = p + d * di1;
}
return true;
}
}
}
//Test si un point est à l'intérieur du cercle.
bool BoundingSphere::isPointInside (math::Vec3f &point) {
float d = center.computeDist(point);
return d <= radius;
}
math::Vec3f BoundingSphere::getPosition() {
return math::Vec3f(center.x - radius, center.y - radius, center.z - radius);
}
math::Vec3f BoundingSphere::getCenter() {
return center;
}
math::Vec3f BoundingSphere::getSize() {
return math::Vec3f(radius * 2, radius * 2, radius * 2);
}
float BoundingSphere::getRadius () {
return radius;
}
void BoundingSphere::scale(float s) {
radius *= s;
}
void BoundingSphere::move(math::Vec3f t) {
center += t;
}
}
} |