bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
{
const btVector3* p1 = &vertices[0];
const btVector3* p2 = &vertices[1];
const btVector3* p3 = &vertices[2];
btVector3 edge1( *p2 - *p1 );
btVector3 edge2( *p3 - *p2 );
btVector3 edge3( *p1 - *p3 );
btVector3 p1_to_p( *p - *p1 );
btVector3 p2_to_p( *p - *p2 );
btVector3 p3_to_p( *p - *p3 );
btVector3 edge1_normal( edge1.cross(normal));
btVector3 edge2_normal( edge2.cross(normal));
btVector3 edge3_normal( edge3.cross(normal));
btScalar r1, r2, r3;
r1 = edge1_normal.dot( p1_to_p );
r2 = edge2_normal.dot( p2_to_p );
r3 = edge3_normal.dot( p3_to_p );
if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
return true;
return false;
}
Vector3 Mesh::faceNormal(const Point3 &p0, const Point3 &p1, const Point3 &p2)
{
//
// Copy your previous (PA04) solution here.
//
// Get two edges (DOUBLE CHECK THIS)
double e0x = p2.u.g.x - p1.u.g.x;
double e0y = p2.u.g.y - p1.u.g.y;
double e0z = p2.u.g.z - p1.u.g.z;
double e1x = p1.u.g.x - p0.u.g.x;
double e1y = p1.u.g.y - p0.u.g.y;
double e1z = p1.u.g.z - p0.u.g.z;
Vector3 edge0(e0x, e0y, e0z);
Vector3 edge1(e1x, e1y, e1z);
// Compute their cross product
Vector3 cross = edge1.cross(edge0);
// Find the normal
//double magnitude = cross.mag();
//Vector3 normal = cross / magnitude;
return cross.normalized(); // permits template to compile cleanly
}
//三角形v0,v1,v2
//判断点v是不是在三角形上只需要知道 O+ Dt = (1 - u - v)V0 + uV1 + vV2
//详细讲解参照：http://www.cnblogs.com/graphics/archive/2010/08/09/1795348.html
float ORay::HitDistance(const OVector3& v0, const OVector3& v1, const OVector3& v2, OVector3* outNormal) const
{
// Based on Fast, Minimum Storage ORay/Triangle Intersection by M鰈ler & Trumbore
// http://www.graphics.cornell.edu/pubs/1997/MT97.pdf
// Calculate edge vectors
OVector3 edge1(v1 - v0);
OVector3 edge2(v2 - v0);
// Calculate determinant & check backfacing
OVector3 p( Vec3_CrossProduct( m_direction, edge2));
float det = Vec3_Dotf( edge1, p );
if (det >= EPSILON)
{
// Calculate u & v parameters and test
OVector3 t(m_origin - v0);
float u = Vec3_Dotf(t, p);
if (u >= 0.0f && u <= det)
{
OVector3 q(Vec3_CrossProduct(t, edge1));
float v = Vec3_Dotf( m_direction, q );
if (v >= 0.0f && u + v <= det)
{
// There is an intersection, so calculate distance & optional normal
if (outNormal)
*outNormal = Vec3_CrossProduct(edge1, edge2);
return Vec3_Dotf(edge2, q) / det;
}
}
}
return INFINITY;
}
// Add an edge from node1 to node2, and from node2 to node1, with
// the given cost. If the cost is < 0, throw a string exception.
void Graph::addEdge(int node1, int node2, double cost)
{
if (cost < 0) {
throw std::string("error: cost is < 0");
}
if (getCost(node1, node2) == -1.0) {
// no edge exists yet, add them
Edge edge1(cost, node2);
Edge edge2(cost, node1);
adjList[node1].edgeList.push_back(edge1);
adjList[node2].edgeList.push_back(edge2);
} else {
// this edge already exists, just update its value
auto iterator = std::find_if(std::begin(adjList[node1].edgeList),
std::end(adjList[node1].edgeList),
[&] (const Edge &edge) {
return edge.dest == node2;
});
iterator->dest = node2;
iterator = std::find_if(std::begin(adjList[node2].edgeList),
std::end(adjList[node2].edgeList),
[&] (const Edge &edge) {
return edge.dest == node1;
});
iterator->dest = node1;
}
}
double
SurfaceOverlapFacet::projected_overlap( SurfaceOverlapFacet &other_facet, CubitBoolean draw_overlap )
{
double tmp_double = agt->ProjectedOverlap( t, other_facet.t, draw_overlap );
if( tmp_double > 0.00 )
{
CubitVector edge0(t.e0.x, t.e0.y, t.e0.z);
CubitVector edge1(t.e1.x, t.e1.y, t.e1.z);
CubitVector normal = edge0 * edge1;
double area_facet1 = normal.length() / 2;
edge0.set(other_facet.t.e0.x, other_facet.t.e0.y, other_facet.t.e0.z);
edge1.set(other_facet.t.e1.x, other_facet.t.e1.y, other_facet.t.e1.z);
normal = edge0 * edge1;
double area_facet2 = normal.length() / 2;
//don't report overlapping area between facets unless it is greater
//than one hundredth of the area of the smaller facet
if( area_facet1 < area_facet2 )
{
if( tmp_double < (area_facet1*0.01))
tmp_double = 0.0;
}
else if( tmp_double < (area_facet2*0.01 ))
tmp_double = 0.0;
}
return tmp_double;
}
float Ray::HitDistance(const Vector3& v0, const Vector3& v1, const Vector3& v2, Vector3* outNormal) const
{
// Based on Fast, Minimum Storage Ray/Triangle Intersection by Möller & Trumbore
// http://www.graphics.cornell.edu/pubs/1997/MT97.pdf
// Calculate edge vectors
Vector3 edge1(v1 - v0);
Vector3 edge2(v2 - v0);
// Calculate determinant & check backfacing
Vector3 p(direction_.CrossProduct(edge2));
float det = edge1.DotProduct(p);
if (det >= M_EPSILON)
{
// Calculate u & v parameters and test
Vector3 t(origin_ - v0);
float u = t.DotProduct(p);
if (u >= 0.0f && u <= det)
{
Vector3 q(t.CrossProduct(edge1));
float v = direction_.DotProduct(q);
if (v >= 0.0f && u + v <= det)
{
// There is an intersection, so calculate distance & optional normal
if (outNormal)
*outNormal = edge1.CrossProduct(edge2);
return edge2.DotProduct(q) / det;
}
}
}
return M_INFINITY;
}
Vector3 Mesh::faceNormal(const Point3 &p0, const Point3 &p1, const Point3 &p2)
{
//
// ASSIGNMENT (PA04)
//
// The points p0, p1, and p2 bound a triangular face in clockwise
// order. Compute two edges, take their cross product, and
// normalize the result to get the face normal, which you return.
//
// 8 lines in instructor solution (YMMV)
//
// Get two edges (DOUBLE CHECK THIS)
double e0x = p2.u.g.x - p1.u.g.x;
double e0y = p2.u.g.y - p1.u.g.y;
double e0z = p2.u.g.z - p1.u.g.z;
double e1x = p1.u.g.x - p0.u.g.x;
double e1y = p1.u.g.y - p0.u.g.y;
double e1z = p1.u.g.z - p0.u.g.z;
Vector3 edge0(e0x, e0y, e0z);
Vector3 edge1(e1x, e1y, e1z);
// Compute their cross product
Vector3 cross = edge1.cross(edge0);
// Find the normal
//double magnitude = cross.mag();
//Vector3 normal = cross / magnitude;
return cross.normalized(); // permits template to compile cleanly
}
void HFaceFormula::calcFaceNormal(HVertex v1, HVertex v2, HVertex v3, HNormal &n) {
HNormal edge1(v1 - v2),
edge2(v2 - v3);
n = edge1 ^ edge2; // cross product
}
plane::plane(const vec3& v1, const vec3& v2, const vec3& v3)
{
vec3 edge1(v2 - v1);
normal = edge1.cross(v3 - v1);
normal.normalize();
d = -normal.dot(v1);
}
Graph::Graph(ifstream& file,float prime_weight)
{
vector<pair<float, float>> vertexs;
int i = 0; //id
string name; //city name
float lat, lon; //latitude & longitude
float weight; //weight = GDP * population
bool prime; //whether it is a prime city
int flag = file.is_open();
string line;
int hz, nj;
while (getline(file,line))
{
stringstream ss;
ss << line;
ss >> name >> lat >> lon >> weight;
pair<float, float> v(lat, lon);
vertexs.push_back(v);
if (weight > prime_weight)
prime = true;
else
prime = false;
City c(i,name, v, weight,prime);
city.push_back(c);
//find hangzhou and nanjing
if (name == "杭州市")
hz = i;
if (name == "南京市")
nj = i;
i++;
}
//construct distances set
//dists[{i,j}] means distance between city with id i and city with id j
for (unsigned i = 0; i < vertexs.size(); ++i)
{
for (unsigned j = 0; j < vertexs.size(); ++j)
{
pair<int, int> edge(i, j);
dists[edge] = dist(vertexs[i], vertexs[j]);
}
}
express[{hz, nj}] = 1; //the amount of express from hz to nj is one car
for (unsigned i = 0; i < vertexs.size(); ++i)
{
for (unsigned j = 0; j < vertexs.size(); ++j)
{
if (i == j) continue;
pair<int, int> edge1(i, j);
pair<int, int> edge2(j, i);
express[edge1] = city[i].weight * city[j].weight / (city[hz].weight * city[nj].weight);
express[edge2] = city[i].weight * city[j].weight / (city[hz].weight * city[nj].weight);
}
}
}
bool PolygonLine :: intersects(Element *element)
{
printf("Warning: entering PolygonLine :: intersects(Element *element).\n");
#ifdef __BOOST_MODULE
if ( !boundingBoxIntersects(element) ) {
return false;
}
double distTol = 1.0e-9;
int numSeg = this->giveNrVertices() - 1;
// Loop over the crack segments and test each segment for
// overlap with the element
for ( int segId = 1; segId <= numSeg; segId++ ) {
if ( element->giveGeometryType() == EGT_triangle_1 ) {
// Crack segment
bPoint2 crackP1( this->giveVertex ( segId )->at(1), this->giveVertex ( segId )->at(2) );
bPoint2 crackP2( this->giveVertex ( segId + 1 )->at(1), this->giveVertex ( segId + 1 )->at(2) );
bSeg2 crackSeg(crackP1, crackP2);
// Triangle vertices
bPoint2 x1( element->giveNode ( 1 )->giveCoordinate(1), element->giveNode ( 1 )->giveCoordinate(2) );
bPoint2 x2( element->giveNode ( 2 )->giveCoordinate(1), element->giveNode ( 2 )->giveCoordinate(2) );
bPoint2 x3( element->giveNode ( 3 )->giveCoordinate(1), element->giveNode ( 3 )->giveCoordinate(2) );
bSeg2 edge1(x1, x2);
bSeg2 edge2(x2, x3);
bSeg2 edge3(x3, x1);
double d1 = bDist(crackSeg, edge1);
if ( d1 < distTol ) {
return true;
}
double d2 = bDist(crackSeg, edge2);
if ( d2 < distTol ) {
return true;
}
double d3 = bDist(crackSeg, edge3);
if ( d3 < distTol ) {
return true;
}
}
}
#endif
return false;
}
void outer( Point3d pentagon[5], Point3d * output) {
for(int i=0; i<5; i++) {
Line3d edge1(pentagon[i], pentagon[(i+1)%5]);
Line3d edge4(pentagon[(i+3)%5], pentagon[(i+4)%5]);
Point3d* ppt = (Point3d*)edge1.intersection(edge4);
if (!ppt) {
std::cout << "INCORRECT!!! diagonal lines are disjoint or identical" << std::endl;
exit(0);
}
output[i] = *ppt;
}
}
float HFaceFormula::calcTriangleFaceArea(HVertex &_v1, HVertex &_v2, HVertex &_v3)
{
ChapillVec3<float> v1(_v1.x, _v1.y, _v1.z),
v2(_v2.x, _v2.y, _v2.z),
v3(_v3.x, _v3.y, _v3.z);
ChapillVec3<float> edge1(v1 - v2),
edge2(v2 - v3);
ChapillVec3<float> normal = edge1 ^ edge2; // cross product
return normal.Length() / 2;
}
Foam::label Foam::removePoints::countPointUsage
(
const scalar minCos,
boolList& pointCanBeDeleted
) const
{
// Containers to store two edges per point:
// -1 : not filled
// >= 0 : edge label
// -2 : more than two edges for point
labelList edge0(mesh_.nPoints(), -1);
labelList edge1(mesh_.nPoints(), -1);
const edgeList& edges = mesh_.edges();
forAll(edges, edgeI)
{
const edge& e = edges[edgeI];
forAll(e, eI)
{
label pointI = e[eI];
if (edge0[pointI] == -2)
{
// Already too many edges
}
else if (edge0[pointI] == -1)
{
// Store first edge using point
edge0[pointI] = edgeI;
}
else
{
// Already one edge using point. Check second container.
if (edge1[pointI] == -1)
{
// Store second edge using point
edge1[pointI] = edgeI;
}
else
{
// Third edge using point. Mark.
edge0[pointI] = -2;
edge1[pointI] = -2;
}
}
}
}
Cloth::Cloth(vec3Array& vertices, std::vector<unsigned int> &indices, const scalarType edgeKs, const scalarType edgeKd, const scalarType bendKs, const scalarType bendKd)
: mVertices(vertices), mIndices(indices)
{
std::map<clothEdge, clothPair> mBendPairs;
for (int i = 0; i < indices.size(); i += 3){
clothSpring s;
s.kd = edgeKd; s.ks = edgeKs;
s.type = EDGE;
// Edge 1
s.p1 = indices[i]; s.p2 = indices[i + 1];
if (mBendPairs.find(clothEdge(s.p1, s.p2)) == mBendPairs.end()){
s.restLen = glm::length(vertices[s.p1] - vertices[s.p2]);
mSprings.push_back(s);
}
// Edge 2
s.p1 = indices[i + 1]; s.p2 = indices[i + 2];
if (mBendPairs.find(clothEdge(s.p1, s.p2)) == mBendPairs.end()){
s.restLen = glm::length(vertices[s.p1] - vertices[s.p2]);
mSprings.push_back(s);
}
// Edge 3
s.p1 = indices[i + 2]; s.p2 = indices[i];
if (mBendPairs.find(clothEdge(s.p1, s.p2)) == mBendPairs.end()){
s.restLen = glm::length(vertices[s.p1] - vertices[s.p2]);
mSprings.push_back(s);
}
clothEdge edge1(indices[i], indices[i + 1]);
mBendPairs[edge1].add(indices[i + 2]);
clothEdge edge2(indices[i + 1], indices[i + 2]);
mBendPairs[edge2].add(indices[i]);
clothEdge edge3(indices[i + 2], indices[i]);
mBendPairs[edge1].add(indices[i + 1]);
}
std::map<clothEdge, clothPair>::iterator iter;
for (iter = mBendPairs.begin(); iter != mBendPairs.end(); iter++){
if (iter->second.isValid()){
clothSpring s;
s.kd = bendKd; s.ks = bendKs;
s.type = BEND;
s.p1 = iter->second.p1; s.p2 = iter->second.p2;
s.restLen = glm::length(vertices[s.p1] - vertices[s.p2]);
mSprings.push_back(s);
}
}
}
void HFaceFormula::calcTriangleFaceFormula(HVertex _v1, HVertex _v2, HVertex _v3)
{
HNormal edge1(_v1 - _v2),
edge2(_v2 - _v3);
HNormal normal = edge1 ^ edge2; // cross product
normal.Normalize();
a = normal.x;
b = normal.y;
c = normal.z;
d = - (a * _v1.x + b * _v1.y + c * _v1.z);
}
bool BSPTree::intersect(Ray &ray, const ISectTri &isecttri, double t_max) const
{
tri_calls++;
// This is the Möller-Trumbore method
Vec3d direction(ray.direction);
Vec3d edge0(isecttri.edge0);
Vec3d edge1(isecttri.edge1);
// Ray-triangle intersection
Vec3d p = cross(direction, edge1);
double a = dot(edge0, p);
if(a > -d_eps && a < d_eps)
return false;
// Just delay these
Vec3d origin(ray.origin);
Vec3d point0(isecttri.point0);
double f = 1.0/a;
Vec3d s = origin - point0;
double u = f*dot(s, p);
if(u < 0.0 || u > 1.0)
return false;
Vec3d q = cross(s, edge0);
double v = f*dot(direction, q);
if(v < 0.0 || u + v > 1.0)
return false;
double t = f*dot(edge1, q);
if(t < f_eps || t*t < 1.0e-9)
return false;
if(t > t_max)
return false;
if(t > ray.dist)
return false;
ray.dist = t;
ray.u = u;
ray.v = v;
ray.hit_object = (TriMesh*)isecttri.mesh_id;
ray.hit_face_id = isecttri.tri_id;
ray.has_hit=true;
return true;
}
/**
* Break edge into several straight line
*
*/
void Edge::breakEdge() {
cv::Point p0; // = contours[0];
cv::Point pend; // = contours[contours.size() - 1];
if (listOfPoints.size() > 0) {
p0 = listOfPoints.at(0);
pend = listOfPoints.at(listOfPoints.size() - 1);
if (listOfPoints.size() > 2) {
Line line(p0, pend);
double distance = 0;
double maxDistance = 0; // ??????????????????
double imax = 0;
for (size_t i = 1; i < listOfPoints.size()-1; i++) {
cv::Point pointi = listOfPoints.at(i);
distance = abs(line.perpendicularDistance(pointi));
if (distance > maxDistance) {
maxDistance = distance;
imax = i;
}
}
if (maxDistance > 3) {
vector<cv::Point> part1(this->listOfPoints.begin(),
this->listOfPoints.begin() + imax + 1);
vector<cv::Point> part2(this->listOfPoints.begin() + imax,
this->listOfPoints.end());
Edge edge1(part1);
Edge edge2(part2);
edge1.breakEdge();
edge2.breakEdge();
}
}
if (!checkPointInList(p0))
breakPoints.push_back(p0);
if (!checkPointInList(pend))
breakPoints.push_back(pend);
} else {
return;
}
}
void Graph::AddEdge(int origin, int direction)
{
bool edge_exist = false;
for(auto edge_b = m_graph[origin].begin(), edge_e = m_graph[origin].end(); edge_b != edge_e ; ++edge_b )
{
if(edge_b->m_direction == direction)
{
edge_exist = true;
}
}
if(edge_exist == false)
{
Edge edge1(origin, direction);
m_graph[origin].push_back(edge1);
edge1.Reverse();
m_graph[direction].push_back(edge1);
}
}
dgFloat32 dgFastRayTest::PolygonIntersect (const dgVector& faceNormal, dgFloat32 maxT, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
dgAssert (m_p0.m_w == dgFloat32 (0.0f));
dgAssert (m_p1.m_w == dgFloat32 (0.0f));
if (faceNormal.DotProduct(m_unitDir).GetScalar() < dgFloat32 (0.0f)) {
dgInt32 stride = dgInt32(strideInBytes / sizeof (dgFloat32));
dgBigVector v0(dgVector(&polygon[indexArray[indexCount - 1] * stride]) & dgVector::m_triplexMask);
dgBigVector p0(m_p0);
dgBigVector p0v0(v0 - p0);
dgBigVector diff(m_diff);
dgBigVector normal(faceNormal);
dgFloat64 tOut = normal.DotProduct(p0v0).GetScalar() / normal.DotProduct(diff).GetScalar();
if ((tOut >= dgFloat64(0.0f)) && (tOut <= maxT)) {
dgBigVector p (p0 + diff.Scale (tOut));
dgBigVector unitDir(m_unitDir);
for (dgInt32 i = 0; i < indexCount; i++) {
dgInt32 i2 = indexArray[i] * stride;
dgBigVector v1(dgVector(&polygon[i2]) & dgVector::m_triplexMask);
dgBigVector edge0(p - v0);
dgBigVector edge1(v1 - v0);
dgFloat64 area = unitDir.DotProduct (edge0.CrossProduct(edge1)).GetScalar();
if (area < dgFloat32 (0.0f)) {
return 1.2f;
}
v0 = v1;
}
return dgFloat32(tOut);
}
}
return dgFloat32 (1.2f);
}
void main()
{
glfwInit();
// Creates a window
window = glfwCreateWindow(800, 800, "OBB - Plane Collision Detection", nullptr, nullptr);
glfwMakeContextCurrent(window);
glfwSwapInterval(0);
// Initializes most things needed before the main loop
init();
//Generate box mesh
struct Vertex boxVerts[24];
boxVerts[0] = { -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Left Corner
boxVerts[1] = { -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Left Corner
boxVerts[2] = { -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Left Corner
boxVerts[3] = { 1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Right Corner
boxVerts[4] = { 1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Right Corner
boxVerts[5] = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Right Corner
boxVerts[6] = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Right Corner
boxVerts[7] = { -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Left Corner
boxVerts[8] = { -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Left Corner
boxVerts[9] = { -1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Top Back Left Corner
boxVerts[10] = { -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Left Corner
boxVerts[11] = { -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Top Front Left Corner
boxVerts[12] = { 1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Right Corner
boxVerts[13] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Top Front Right Corner
boxVerts[14] = { 1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Right Corner
boxVerts[15] = { 1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Top Back Right Corner
boxVerts[16] = { -1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Left Corner
boxVerts[17] = { -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Left Corner
boxVerts[18] = { -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Left Corner
boxVerts[19] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Right Corner
boxVerts[20] = { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Front Right Corner
boxVerts[21] = { 1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Right Corner
boxVerts[22] = { 1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Right Corner
boxVerts[23] = { -1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f }; //Bottom Back Left Corner
box = new struct Mesh(24, boxVerts, GL_LINES);
//Scale the box
box->scale = box->scale * glm::scale(glm::mat4(1.0f), glm::vec3(0.1f, 0.1f, 0.1f));
//Translate the box
box->translation = glm::translate(box->translation, glm::vec3(-0.1f, 0.0f, 0.0f));
//Generate the Plane mesh
struct Vertex planeVerts[6];
planeVerts[0] = { 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[1] = { 0.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[2] = { 0.0f, -1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[3] = { 0.0f, -1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[4] = { 0.0f, 1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[5] = { 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
plane = new struct Mesh(6, planeVerts, GL_TRIANGLES);
//Scale the plane
plane->scale = plane->scale * glm::scale(glm::mat4(1.0f), glm::vec3(5.0f, 5.0f, 5.0f));
//Translate the plane
plane->translation = glm::translate(plane->translation, glm::vec3(0.1f, 0.0f, 0.0f));
//Set the selected shape
selectedShape = plane;
//Generate the colliders
boxCollider = new struct OBB(boxVerts[3].x - boxVerts[2].x, boxVerts[9].y - boxVerts[8].y, boxVerts[1].z - boxVerts[0].z);
//Get two edges of the plane and take the cross product for the normal (Or just hardcode it, for example we know the normal to this plane
//Will be the Z axis, because the plane mesh lies in the XY Plane to start.
glm::vec3 edge1(planeVerts[0].x - planeVerts[1].x, planeVerts[0].y - planeVerts[1].y, planeVerts[0].z - planeVerts[1].z);
glm::vec3 edge2(planeVerts[1].x - planeVerts[2].x, planeVerts[1].y - planeVerts[2].y, planeVerts[1].z - planeVerts[2].z);
glm::vec3 normal = glm::normalize(glm::cross(edge1, edge2));
planeCollider = new struct Plane(normal);
//Print controls
std::cout << "Use WASD to move the selected shape in the XY plane.\nUse left CTRL & left shift to move the selected shape along Z axis.\n";
std::cout << "Left click and drag the mouse to rotate the selected shape.\nUse spacebar to swap the selected shape.\n";
// Enter the main loop.
while (!glfwWindowShouldClose(window))
{
// Call to update() which will update the gameobjects.
update();
// Call the render function.
renderScene();
// Swaps the back buffer to the front buffer
glfwSwapBuffers(window);
// Checks to see if any events are pending and then processes them.
glfwPollEvents();
}
// After the program is over, cleanup your data!
glDeleteShader(vertex_shader);
glDeleteShader(fragment_shader);
glDeleteProgram(program);
// Note: If at any point you stop using a "program" or shaders, you should free the data up then and there.
delete box;
delete plane;
delete boxCollider;
delete planeCollider;
// Frees up GLFW memory
glfwTerminate();
}
bool CTriangulation::Triangulate(vector<CEdgeLoop> &tri_faces, CRegion& region)
{
vector<CEdge> regionEdges;
for(EdgeArray::iterator ol_it = region.OuterLoops.Edges.begin();ol_it != region.OuterLoops.Edges.end();ol_it++)
{
regionEdges.push_back(*ol_it);
}
for(vector<CEdgeLoop>::iterator il_it1 = region.InnerLoops.begin();il_it1 != region.InnerLoops.end();il_it1++)
{
CEdgeLoop currentInnerLoop = *il_it1;
for(EdgeArray::iterator il_it2 = currentInnerLoop.Edges.begin();il_it2 != currentInnerLoop.Edges.end();il_it2++)
{
regionEdges.push_back(*il_it2);
}
}
while(regionEdges.size() > 3)
{
vector<vec2> candidatePoints;
for(int i = 1;i<regionEdges.size();i++)
{
if (CMathUtility::ToLeftExcludeOnLine(regionEdges[i].End,regionEdges[0]))
{
int k = 0;
for(k = 1;k<regionEdges.size();k++)
{
CEdge edge1(regionEdges[0].Start,regionEdges[i].End);
edge1.Commit();
CEdge edge2(regionEdges[0].End,regionEdges[i].End);
edge2.Commit();
//???????
if(edge1.IsIntersectWith2(regionEdges[k]) || edge2.IsIntersectWith2(regionEdges[k]))
{
break;
}
}
if(k == regionEdges.size())
{
candidatePoints.push_back(regionEdges[i].End);
}
}
}
double minR = MAXDWORD;
vec2 LO2;
for(int j = 0;j<candidatePoints.size();j++)
{
double r =CMathUtility::MinR(regionEdges[0].Start,regionEdges[0].End,candidatePoints[j]);
if(r < minR)
{
minR = r;
LO2 = candidatePoints[j];
}
}
CEdgeLoop tmpTriangel;
tmpTriangel.Edges.push_back(regionEdges[0]);
CEdge tmpEdge1(regionEdges[0].End,LO2);
tmpEdge1.Commit();
tmpTriangel.Edges.push_back(tmpEdge1);
CEdge tmpEdge2(LO2,regionEdges[0].Start);
tmpEdge2.Commit();
tmpTriangel.Edges.push_back(tmpEdge2);
tmpTriangel.Commit();
vector<vec2> newCandidatePoints;
for(int m = 0;m<candidatePoints.size();m++)
{
if(CMathUtility::IsCanExistInCircle(candidatePoints[m],tmpTriangel))
{
newCandidatePoints.push_back(candidatePoints[m]);
}
}
while (!newCandidatePoints.size() ==0)
{
minR=MAXDWORD;
for(int m = 0;m < newCandidatePoints.size();m++)
{
double r=CMathUtility::MinR(regionEdges[0].Start,regionEdges[0].End,newCandidatePoints[m]);
if(r < minR)
{
minR = r;
LO2 = newCandidatePoints[m];
}
}
tmpTriangel.Edges.clear();
tmpTriangel.Edges.push_back(regionEdges[0]);
tmpEdge1.Start = regionEdges[0].End;
tmpEdge1.End = LO2;
tmpTriangel.Edges.push_back(tmpEdge1);
tmpEdge2.Start = LO2;
tmpEdge2.End = regionEdges[0].Start;
tmpTriangel.Edges.push_back(tmpEdge2);
tmpTriangel.Commit();
newCandidatePoints.clear();
for (int j = 0;j<candidatePoints.size();j++)
{
if (CMathUtility::IsCanExistInCircle(candidatePoints[j],tmpTriangel))
{
newCandidatePoints.push_back(candidatePoints[j]);
}
}
}
tri_faces.push_back(tmpTriangel);
CEdge L11_LO2(regionEdges[0].Start,LO2);
L11_LO2.isBoundEdge = false;
L11_LO2.Commit();
CEdge L12_LO2(LO2,regionEdges[0].End);
L12_LO2.isBoundEdge =false;
L12_LO2.Commit();
int pos_m = 0;
int pos_n = 0;
if(CMathUtility::IsBoundEdge(regionEdges,L11_LO2,pos_m))
{
L11_LO2.isBoundEdge = true;
}
if(CMathUtility::IsBoundEdge(regionEdges,L12_LO2,pos_n))
{
L12_LO2.isBoundEdge = true;
}
if(!L11_LO2.isBoundEdge && !L12_LO2.isBoundEdge)
{
CEdge tmpEdge(L12_LO2.Start,L12_LO2.End);
tmpEdge.Commit();
regionEdges.push_back(tmpEdge);
regionEdges[0].End = LO2;
regionEdges[0].Commit();
}
if(L11_LO2.isBoundEdge && !L12_LO2.isBoundEdge)
{
regionEdges[0].Start = LO2;
regionEdges[0].Commit();
regionEdges[pos_m] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
}
if(!L11_LO2.isBoundEdge && L12_LO2.isBoundEdge)
{
regionEdges[0].End = LO2;
regionEdges[0].Commit();
regionEdges[pos_n] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
}
if(L11_LO2.isBoundEdge && L12_LO2.isBoundEdge)
{
if (pos_n>pos_m)
{
regionEdges[pos_n] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
regionEdges[pos_m] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
regionEdges[0] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
}
else
{
regionEdges[pos_m] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
regionEdges[pos_n] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
regionEdges[0] = regionEdges[regionEdges.size()-1];
regionEdges.pop_back();
}
}
}
// TODO
if(regionEdges.size()==3)
{
CEdgeLoop tmpTriangle;
tmpTriangle.Edges.push_back(regionEdges[0]);
tmpTriangle.Edges.push_back(regionEdges[1]);
tmpTriangle.Edges.push_back(regionEdges[2]);
tmpTriangle.Commit();
tri_faces.push_back(tmpTriangle);
}
return true;
}
void ellipseTransform(QPointF p1, QPointF p2, qreal rx, qreal ry, int largeFlag, int sweepFlag, int angel,
qreal* startAngle, qreal *sweepLength, QRectF *rect){
qreal r1 = (p1.x() - p2.x()) / (2.f * rx);
qreal r2 = (p2.y() - p1.y()) / (2.f * ry);
qreal tmp = atan(r1 / r2);
qreal a1s[2] = { tmp, -tmp };
tmp = asin(sqrt(r1*r1 + r2*r2));
qreal a2s[2] = { tmp, -tmp };
qreal x, y, t1, t2;
qreal rrmin = rx*rx;
qreal rrmax = ry*ry;
if (rrmin > rrmax){
tmp = rrmin;
rrmin = rrmax;
rrmax = tmp;
}
for (int i = 0; i < 2;i++)
{
qreal a1 = a1s[i];
bool isBreak = false;
for (int j = 0; j < 2; j++)
{
qreal a2 = a2s[j];
t1 = a1 + a2;
t2 = a1 - a2;
x = p1.x() - rx*cos(t1);
y = p1.y() - ry*sin(t1);
qreal check1 = (p1.x() - x)*(p1.x() - x) / (rx*rx) + (p1.y() - y)*(p1.y() - y) / (ry*ry);
qreal check2 = (p2.x() - x)*(p2.x() - x) / (rx*rx) + (p2.y() - y)*(p2.y() - y) / (ry*ry);
if ( fabs(check1-1) < 1.e-6 && fabs(check2-1) < 1.e-6)
{
isBreak = true;
break;
}
}
if (isBreak) break;
}
QVector3D edge1(x - p1.x(), y - p1.y(),0);
QVector3D edge2(p2.x() - x, p2.y() - y,0);
QVector3D cross = QVector3D::crossProduct(edge1,edge2);
bool isCw = cross.z() > 0;
if (isCw != (largeFlag == sweepFlag))
{
QPointF centerOfp1p2 = (p1 + p2) / 2.f;
x = centerOfp1p2.x() * 2 - x;
y = centerOfp1p2.y() * 2 - y;
/*x = x0 + a * cos(t)
y = y0 + b * sin(t)*/
}
*rect = QRect(x - rx, y - ry, rx * 2, ry * 2);
t1 = atan2((p1.y() - y) / ry, (p1.x() - x) / rx);
t2 = atan2((p2.y() - y) / ry, (p2.x() - x) / rx);
*startAngle = t1 * 180 / PI;
*startAngle = -*startAngle;
*sweepLength = t2 * 180 / PI;
*sweepLength = -*sweepLength;
*sweepLength -= *startAngle;
if ((largeFlag && fabs(*sweepLength) < 180) || (!largeFlag && fabs(*sweepLength) > 180))
{
if (*sweepLength < 0)
*sweepLength = -360 - *sweepLength;
else
*sweepLength = 360 - *sweepLength;
}
if ((!sweepFlag && *sweepLength <0) || (sweepFlag && *sweepLength >0))
{
*sweepLength = -*sweepLength;
}
/*if (!isCw)
{
*sweepLength = fabs(*sweepLength);
}*/
}
hacd::HaI32 dgPolygonSoupDatabaseBuilder::FilterFace (hacd::HaI32 count, hacd::HaI32* const pool)
{
if (count == 3) {
dgBigVector p0 (m_vertexPoints[pool[2]]);
for (hacd::HaI32 i = 0; i < 3; i ++) {
dgBigVector p1 (m_vertexPoints[pool[i]]);
dgBigVector edge (p1 - p0);
hacd::HaF64 mag2 = edge % edge;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
count = 0;
}
p0 = p1;
}
if (count == 3) {
dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal (edge0 * edge1);
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
count = 0;
}
}
} else {
dgPolySoupFilterAllocator polyhedra;
count = polyhedra.AddFilterFace (hacd::HaU32 (count), pool);
if (!count) {
return 0;
}
dgEdge* edge = &polyhedra.GetRoot()->GetInfo();
if (edge->m_incidentFace < 0) {
edge = edge->m_twin;
}
bool flag = true;
while (flag) {
flag = false;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
do {
dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
hacd::HaF64 mag2 = e0 % e0;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
p0 = p1;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
if (count >= 3) {
flag = true;
dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
HACD_ASSERT ((normal % normal) > hacd::HaF32 (1.0e-10f));
normal = normal.Scale (dgRsqrt (normal % normal + hacd::HaF32 (1.0e-20f)));
while (flag) {
flag = false;
if (count >= 3) {
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_prev->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
hacd::HaF64 mag2 = e1 % e0;
if (mag2 > hacd::HaF32 (0.9999f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
dgBigVector n (e0 * e1);
mag2 = n % normal;
if (mag2 < hacd::HaF32 (1.0e-5f)) {
count --;
flag = true;
edge = ptr->m_next;
ptr->m_prev->m_next = ptr->m_next;
ptr->m_next->m_prev = ptr->m_prev;
ptr->m_twin->m_next->m_prev = ptr->m_twin->m_prev;
ptr->m_twin->m_prev->m_next = ptr->m_twin->m_next;
break;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
}
}
}
dgEdge* first = edge;
if (count >= 3) {
hacd::HaF64 best = hacd::HaF32 (2.0f);
dgEdge* ptr = edge;
dgBigVector p0 (&m_vertexPoints[ptr->m_incidentVertex].m_x);
dgBigVector p1 (&m_vertexPoints[ptr->m_next->m_incidentVertex].m_x);
dgBigVector e0 (p1 - p0);
e0 = e0.Scale (dgRsqrt (e0 % e0 + hacd::HaF32(1.0e-10f)));
do {
dgBigVector p2 (&m_vertexPoints[ptr->m_next->m_next->m_incidentVertex].m_x);
dgBigVector e1 (p2 - p1);
e1 = e1.Scale (dgRsqrt (e1 % e1 + hacd::HaF32(1.0e-10f)));
hacd::HaF64 mag2 = fabs (e1 % e0);
if (mag2 < best) {
best = mag2;
first = ptr;
}
e0 = e1;
p1 = p2;
ptr = ptr->m_next;
} while (ptr != edge);
count = 0;
ptr = first;
do {
pool[count] = ptr->m_incidentVertex;
count ++;
ptr = ptr->m_next;
} while (ptr != first);
}
#ifdef _DEBUG
if (count >= 3) {
hacd::HaI32 j0 = count - 2;
hacd::HaI32 j1 = count - 1;
dgBigVector normal (polyhedra.FaceNormal (edge, &m_vertexPoints[0].m_x, sizeof (dgBigVector)));
for (hacd::HaI32 j2 = 0; j2 < count; j2 ++) {
dgBigVector p0 (&m_vertexPoints[pool[j0]].m_x);
dgBigVector p1 (&m_vertexPoints[pool[j1]].m_x);
dgBigVector p2 (&m_vertexPoints[pool[j2]].m_x);
dgBigVector e0 ((p0 - p1));
dgBigVector e1 ((p2 - p1));
dgBigVector n (e1 * e0);
HACD_ASSERT ((n % normal) > hacd::HaF32 (0.0f));
j0 = j1;
j1 = j2;
}
}
#endif
}
return (count >= 3) ? count : 0;
}
void dgPolygonSoupDatabaseBuilder::AddMesh (const hacd::HaF32* const vertex, hacd::HaI32 vertexCount, hacd::HaI32 strideInBytes, hacd::HaI32 faceCount,
const hacd::HaI32* const faceArray, const hacd::HaI32* const indexArray, const hacd::HaI32* const faceTagsData, const dgMatrix& worldMatrix)
{
hacd::HaI32 faces[256];
hacd::HaI32 pool[2048];
m_vertexPoints[m_vertexCount + vertexCount].m_x = hacd::HaF64 (0.0f);
dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];
worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
vertexPool[i].m_w = hacd::HaF64 (0.0f);
}
hacd::HaI32 totalIndexCount = faceCount;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
totalIndexCount += faceArray[i];
}
m_vertexIndex[m_indexCount + totalIndexCount] = 0;
m_faceVertexCount[m_faceCount + faceCount] = 0;
hacd::HaI32 k = 0;
for (hacd::HaI32 i = 0; i < faceCount; i ++) {
hacd::HaI32 count = faceArray[i];
for (hacd::HaI32 j = 0; j < count; j ++) {
hacd::HaI32 index = indexArray[k];
pool[j] = index + m_vertexCount;
k ++;
}
hacd::HaI32 convexFaces = 0;
if (count == 3) {
convexFaces = 1;
dgBigVector p0 (m_vertexPoints[pool[2]]);
for (hacd::HaI32 i = 0; i < 3; i ++) {
dgBigVector p1 (m_vertexPoints[pool[i]]);
dgBigVector edge (p1 - p0);
hacd::HaF64 mag2 = edge % edge;
if (mag2 < hacd::HaF32 (1.0e-6f)) {
convexFaces = 0;
}
p0 = p1;
}
if (convexFaces) {
dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
dgBigVector normal (edge0 * edge1);
hacd::HaF64 mag2 = normal % normal;
if (mag2 < hacd::HaF32 (1.0e-8f)) {
convexFaces = 0;
}
}
if (convexFaces) {
faces[0] = 3;
}
} else {
convexFaces = AddConvexFace (count, pool, faces);
}
hacd::HaI32 index = 0;
for (hacd::HaI32 k = 0; k < convexFaces; k ++) {
hacd::HaI32 count = faces[k];
m_vertexIndex[m_indexCount] = faceTagsData[i];
m_indexCount ++;
for (hacd::HaI32 j = 0; j < count; j ++) {
m_vertexIndex[m_indexCount] = pool[index];
index ++;
m_indexCount ++;
}
m_faceVertexCount[m_faceCount] = count + 1;
m_faceCount ++;
}
}
m_vertexCount += vertexCount;
m_run -= vertexCount;
if (m_run <= 0) {
PackArray();
}
}
Subdivide (int _vertexCount, glm::vec3* _vertices, int _triangleCount, glm::ivec3* _triangles, int _subCount)
:vertexCount(_vertexCount), triangleCount(_triangleCount * _subCount * _subCount), subCount(_subCount)
{
vertices.resize(vertexCount);
triangles.resize(triangleCount);
for (int i = 0; i < vertexCount; ++i){
vertices[i] = _vertices[i];
}
std::map<SubdivideEdge, std::vector<int> > edgeMap;
int triIndex = 0;
for (int i = 0; i < _triangleCount; ++i){
std::vector<int> triIndices;
// Edge 1
SubdivideEdge edge1(_triangles[i].x, _triangles[i].y);
if (edgeMap.find(edge1) == edgeMap.end()){
glm::vec3 vertexGap = (vertices[edge1.e2] - vertices[edge1.e1]) / (float)subCount;
for (int i = 1; i < subCount; ++i){
vertices.push_back(vertices[edge1.e1] + vertexGap * (float)i);
edgeMap[edge1].push_back(vertexCount++);
}
}
if (edge1.isExchanged){
triIndices.push_back(edge1.e2);
const std::vector<int>& tmpI = edgeMap[edge1];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[tmpI.size() - i - 1]);
}
}else{
triIndices.push_back(edge1.e1);
const std::vector<int>& tmpI = edgeMap[edge1];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[i]);
}
}
// Edge 2
SubdivideEdge edge2(_triangles[i].y, _triangles[i].z);
if (edgeMap.find(edge2) == edgeMap.end()){
glm::vec3 vertexGap = (vertices[edge2.e2] - vertices[edge2.e1]) / (float)subCount;
for (int i = 1; i < subCount; ++i){
vertices.push_back(vertices[edge2.e1] + vertexGap * (float)i);
edgeMap[edge2].push_back(vertexCount++);
}
}
if (edge2.isExchanged){
triIndices.push_back(edge2.e2);
const std::vector<int>& tmpI = edgeMap[edge2];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[tmpI.size() - i - 1]);
}
}else{
triIndices.push_back(edge2.e1);
const std::vector<int>& tmpI = edgeMap[edge2];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[i]);
}
}
// Edge 3
SubdivideEdge edge3(_triangles[i].z, _triangles[i].x);
if (edgeMap.find(edge3) == edgeMap.end()){
glm::vec3 vertexGap = (vertices[edge3.e2] - vertices[edge3.e1]) / (float)subCount;
for (int i = 1; i < subCount; ++i){
vertices.push_back(vertices[edge3.e1] + vertexGap * (float)i);
edgeMap[edge3].push_back(vertexCount++);
}
}
if (edge3.isExchanged){
triIndices.push_back(edge3.e2);
const std::vector<int>& tmpI = edgeMap[edge3];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[tmpI.size() - i - 1]);
}
}else{
triIndices.push_back(edge3.e1);
const std::vector<int>& tmpI = edgeMap[edge3];
for (int i = 0; i < tmpI.size(); ++i){
triIndices.push_back(tmpI[i]);
}
}
// Limit the subCount to [2, 5]
subCount = subCount < 2 ? 2 : subCount;
subCount = subCount > 5 ? 5 : subCount;
// Create vertices inner triangle
int currTriIndicesCnt = triIndices.size();
for (int step = 2; step < subCount; ++step){
glm::vec3 vertexGap = (vertices[triIndices[currTriIndicesCnt - step]] - vertices[triIndices[step]]) / (float)step;
for (int i = 1; i < step; ++i){
vertices.push_back(vertices[triIndices[step]] + vertexGap * (float)i);
triIndices.push_back(vertexCount++);
}
}
// Create triangle index
int size = subCount * subCount;
for (int i = 0; i < size; i++){
triangles[triIndex++] = glm::ivec3(triIndices[subDivideMap[subCount][i * 3]],
triIndices[subDivideMap[subCount][i * 3 + 1]],
triIndices[subDivideMap[subCount][i * 3 + 2]]);
}
}
}
void main()
{
glfwInit();
// Creates a window
window = glfwCreateWindow(800, 800, "Point - Plane Collision Detection", nullptr, nullptr);
glfwMakeContextCurrent(window);
glfwSwapInterval(0);
// Initializes most things needed before the main loop
init();
//Generate the Plane1 mesh
struct Vertex planeVerts[6];
planeVerts[0] = { 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[1] = { 0.0f, -1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[2] = { 0.0f, -1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[3] = { 0.0f, -1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[4] = { 0.0f, 1.0f, -1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
planeVerts[5] = { 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 1.0f, 1.0f };
plane = new struct Mesh(6, planeVerts, GL_TRIANGLES);
//Translate the plane
plane->translation = glm::translate(plane->translation, glm::vec3(0.15f, 0.0f, 0.0f));
//Generate point mesh
struct Vertex pointVert = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 1.0f };
point = new struct Mesh(1, &pointVert, GL_POINTS);
//Translate the plane
point->translation = glm::translate(point->translation, glm::vec3(-0.15f, 0.0f, 0.0f));
//Set the selected shape
selectedShape = plane;
//Generate plane collider
//Get two edges of the plane and take the cross product for the normal (Or just hardcode it, for example we know the normal to this plane
//Will be the Z axis, because the plane mesh lies in the XY Plane to start.
glm::vec3 edge1(planeVerts[0].x - planeVerts[1].x, planeVerts[0].y - planeVerts[1].y, planeVerts[0].z - planeVerts[1].z);
glm::vec3 edge2(planeVerts[1].x - planeVerts[2].x, planeVerts[1].y - planeVerts[2].y, planeVerts[1].z - planeVerts[2].z);
glm::vec3 normal = glm::normalize(glm::cross(edge1, edge2));
planeCollider = new struct Plane(normal);
//Print controls
std::cout << "Use WASD to move the selected shape in the XY plane.\nUse left CTRL & left shift to move the selected shape along Z axis.\n";
std::cout << "Left click and drag the mouse to rotate the selected shape.\nUse spacebar to swap the selected shape.\n";
// Enter the main loop.
while (!glfwWindowShouldClose(window))
{
// Call to update() which will update the gameobjects.
update();
// Call the render function.
renderScene();
// Swaps the back buffer to the front buffer
glfwSwapBuffers(window);
// Checks to see if any events are pending and then processes them.
glfwPollEvents();
}
// After the program is over, cleanup your data!
glDeleteShader(vertex_shader);
glDeleteShader(fragment_shader);
glDeleteProgram(program);
delete plane;
delete point;
delete planeCollider;
// Frees up GLFW memory
glfwTerminate();
}
/*
Convert tets and pyramids next to close (identified) points into prisms
*/
void MakePrismsClosePoints (Mesh & mesh)
{
int i, j, k;
for (i = 1; i <= mesh.GetNE(); i++)
{
Element & el = mesh.VolumeElement(i);
if (el.GetType() == TET)
{
for (j = 1; j <= 3; j++)
for (k = j+1; k <= 4; k++)
{
INDEX_2 edge(el.PNum(j), el.PNum(k));
edge.Sort();
if (mesh.GetIdentifications().GetSymmetric (el.PNum(j), el.PNum(k)))
{
int pi3 = 1, pi4 = 1;
while (pi3 == j || pi3 == k) pi3++;
pi4 = 10 - j - k - pi3;
int p3 = el.PNum(pi3);
int p4 = el.PNum(pi4);
el.SetType(PRISM);
el.PNum(1) = edge.I1();
el.PNum(2) = p3;
el.PNum(3) = p4;
el.PNum(4) = edge.I2();
el.PNum(5) = p3;
el.PNum(6) = p4;
}
}
}
if (el.GetType() == PYRAMID)
{
// pyramid, base face = 1,2,3,4
for (j = 0; j <= 1; j++)
{
int pi1 = el.PNum( (j+0) % 4 + 1);
int pi2 = el.PNum( (j+1) % 4 + 1);
int pi3 = el.PNum( (j+2) % 4 + 1);
int pi4 = el.PNum( (j+3) % 4 + 1);
int pi5 = el.PNum(5);
INDEX_2 edge1(pi1, pi4);
INDEX_2 edge2(pi2, pi3);
edge1.Sort();
edge2.Sort();
if (mesh.GetIdentifications().GetSymmetric (pi1, pi4) &&
mesh.GetIdentifications().GetSymmetric (pi2, pi3))
{
//int p3 = el.PNum(pi3);
//int p4 = el.PNum(pi4);
el.SetType(PRISM);
el.PNum(1) = pi1;
el.PNum(2) = pi2;
el.PNum(3) = pi5;
el.PNum(4) = pi4;
el.PNum(5) = pi3;
el.PNum(6) = pi5;
}
}
}
}
for (i = 1; i <= mesh.GetNSE(); i++)
{
Element2d & el = mesh.SurfaceElement(i);
if (el.GetType() != TRIG) continue;
for (j = 1; j <= 3; j++)
{
k = (j % 3) + 1;
INDEX_2 edge(el.PNum(j), el.PNum(k));
edge.Sort();
if (mesh.GetIdentifications().GetSymmetric (el.PNum(j), el.PNum(k)))
{
int pi3 = 6-j-k;
int p3 = el.PNum(pi3);
int p1 = el.PNum(j);
int p2 = el.PNum(k);
el.SetType(QUAD);
el.PNum(1) = p2;
el.PNum(2) = p3;
el.PNum(3) = p3;
el.PNum(4) = p1;
}
}
}
}
bool DivideAndConquerFor3DCH::RayTriangleIntersection( Ray r, TRIANGLE triangle, const vector<VERTEX*>* pVertex )
{
VERTEX* pointOne = (*pVertex)[ triangle.p1.pointOneIndex ];
VERTEX* pointTwo = (*pVertex)[ triangle.p2.pointTwoIndex ];
VERTEX* pointThree = (*pVertex)[ triangle.p3.pointThreeIndex ];
D3DXVECTOR3 edge1( pointTwo->x - pointOne->x, pointTwo->y - pointOne->y, pointTwo->z - pointOne->z );
D3DXVECTOR3 edge2( pointThree->x - pointOne->x, pointThree->y - pointOne->y, pointThree->z - pointOne->z );
D3DXVECTOR3 triNormal;
D3DXVec3Cross( &triNormal, &edge1, &edge2 );
D3DXVec3Normalize( &triNormal, &triNormal );
double denominator = D3DXVec3Dot( &triNormal, &r.direction );
// Ray parallels to the plane
if( fabs( denominator ) < 0.000001 )
{
return false;
}
double d = triNormal.x * pointOne->x + triNormal.y * pointOne->y + triNormal.z * pointOne->z;
double t = ( d - D3DXVec3Dot( &triNormal, &r.position ) ) / denominator;
// Trianle behine the ray
if( t <= 0 )
{
return false;
}
D3DXVECTOR3 intersectPoint = r.position + t * r.direction;
//D3DXVECTOR3 tmp;
//D3DXVec3Cross( &tmp, &edge1, &edge2 );
//double totalAmount = D3DXVec3Dot( &tmp, &triNormal );
//double totalArea = D3DXVec3Length( &tmp ) * 0.5;
//VERTEX tmpV = pointThree - pointTwo;
//D3DXVec3Cross( &tmp, &D3DXVECTOR3( tmpV.x, tmpV.y, tmpV.z ), &D3DXVECTOR3( intersectPoint.x - pointTwo.x, intersectPoint.y - pointTwo.y, intersectPoint.z - pointTwo.z ) );
//double alpha = D3DXVec3Length( &tmp ) * 0.5 / totalArea;
//
//tmpV = pointOne - pointThree;
//D3DXVec3Cross( &tmp, &D3DXVECTOR3( tmpV.x, tmpV.y, tmpV.z ), &D3DXVECTOR3( intersectPoint.x - pointThree.x, intersectPoint.y - pointThree.y, intersectPoint.z - pointThree.z ) );
//double beta = D3DXVec3Length( &tmp ) * 0.5 / totalArea;
//tmpV = pointTwo - pointOne;
//D3DXVec3Cross( &tmp, &D3DXVECTOR3( tmpV.x, tmpV.y, tmpV.z ), &D3DXVECTOR3( intersectPoint.x - pointOne.x, intersectPoint.y - pointOne.y, intersectPoint.z - pointOne.z ) );
//double gamma = D3DXVec3Length( &tmp ) * 0.5 / totalArea;
// if( alpha + beta + gamma > 1.00001 )
// {
// return false;
// }
D3DXVECTOR3 tmpEdge( intersectPoint.x - pointOne->x, intersectPoint.y - pointOne->y, intersectPoint.z - pointOne->z );
D3DXVECTOR3 tmpCrossRes;
D3DXVec3Cross( &tmpCrossRes, &edge1, &tmpEdge );
double alpha = D3DXVec3Dot( &triNormal, &tmpCrossRes );
if( alpha < 0.0f )
{
return false;
}
tmpEdge = D3DXVECTOR3( intersectPoint.x - pointTwo->x, intersectPoint.y - pointTwo->y, intersectPoint.z - pointTwo->z);
D3DXVECTOR3 tmpEdge2( pointThree->x - pointTwo->x, pointThree->y - pointTwo->y, pointThree->z - pointTwo->z );
D3DXVec3Cross( &tmpCrossRes, &tmpEdge2, &tmpEdge );
double beta = D3DXVec3Dot( &triNormal, &tmpCrossRes );
if( beta < 0.0f )
{
return false;
}
tmpEdge = D3DXVECTOR3( intersectPoint.x - pointThree->x, intersectPoint.y - pointThree->y, intersectPoint.z - pointThree->z );
tmpEdge2 = D3DXVECTOR3( pointOne->x - pointThree->x, pointOne->y - pointThree->y, pointOne->z - pointThree->z );
D3DXVec3Cross( &tmpCrossRes, &tmpEdge2, &tmpEdge );
double gamma = D3DXVec3Dot( &triNormal, &tmpCrossRes );
if( gamma < 0.0f )
{
return false;
}
return true;
}
DCEL DivideAndConquerFor3DCH::BruceForceCH( vector<VERTEX*>* pVertex, const unsigned int offset )
{
vector<TRIANGLE> triangleSet, finalTriangleSet;
// Generate all possible triangles
int pointSetSize = pVertex->size();
for( int i = 0; i < pointSetSize; i++ )
{
for( int j = i + 1; j < pointSetSize; j++ )
{
for( int k = j + 1; k < pointSetSize; k++ )
{
// Forming face
TRIANGLE face;
face.p1.pointOneIndex = i;
face.p2.pointTwoIndex = j;
face.p3.pointThreeIndex = k;
triangleSet.push_back( face );
}
}
}
// Find the CH for this point set by using RayAndTriangleIntersection method
for( int i = 0; i < triangleSet.size(); i++ )
{
// Create a ray from this surface
TRIANGLE triangle = triangleSet[ i ];
// Point2 - point1
//D3DXVECTOR3 edge1( triangle.pointTwo.x - triangle.pointOne.x, triangle.pointTwo.y - triangle.pointOne.y, triangle.pointTwo.z - triangle.pointOne.z );
VERTEX* pointOne = (*pVertex)[ triangle.p1.pointOneIndex ];
VERTEX* pointTwo = (*pVertex)[ triangle.p2.pointTwoIndex ];
VERTEX* pointThree = (*pVertex)[ triangle.p3.pointThreeIndex ];
D3DXVECTOR3 edge1( pointTwo->x - pointOne->x, pointTwo->y - pointOne->y, pointTwo->z - pointOne->z );
D3DXVECTOR3 edge2( pointThree->x - pointOne->x, pointThree->y - pointOne->y, pointThree->z - pointOne->z );
// point3 - point1
//D3DXVECTOR3 edge2( triangle.pointThree.x - triangle.pointOne.x, triangle.pointThree.y - triangle.pointOne.y, triangle.pointThree.z - triangle.pointOne.z );
D3DXVECTOR3 triangleNormal;
D3DXVec3Cross( &triangleNormal, &edge1, &edge2 );
D3DXVECTOR3 rayStartPoint( ( pointOne->x + pointTwo->x + pointThree->x ) / 3.0f,
( pointOne->y + pointTwo->y + pointThree->y ) / 3.0f,
( pointOne->z + pointTwo->z + pointThree->z ) / 3.0f );
Ray ray, invRay;
D3DXVec3Normalize( &ray.direction, &triangleNormal );
ray.position = rayStartPoint;
invRay = ray;
invRay.direction *= -1.0;
bool rayIntersect = !isNormal(ray, triangleSet[i], pVertex);
bool invRayIntersect = !isNormal(invRay, triangleSet[i], pVertex);
// This is the face that contribute to the convex hull and find its vertices order
if( rayIntersect == false && invRayIntersect == true )
{
finalTriangleSet.push_back( triangleSet[ i ] );
}
else if( rayIntersect == true && invRayIntersect == false )
{
TRIANGLE tmpTri = triangleSet[ i ];
int tmpVerIndex = tmpTri.p2.pointTwoIndex;
tmpTri.p2.pointTwoIndex = tmpTri.p3.pointThreeIndex;
tmpTri.p3.pointThreeIndex = tmpVerIndex;
finalTriangleSet.push_back( tmpTri );
}
}
DCEL dcel;
dcel.createDCEL( &finalTriangleSet, pVertex, offset );
return dcel;
}
