pathfinding change: the engine uses Triangulation and A* on triangles now. dcdt package added. premake.lua changed to include the dcdt code. it needs to run update-workspaces.bat (flag -showOverlay will draw the triangulation and a single unit paths)
This was SVN commit r5393.
This commit is contained in:
kai
parent
4b7713fd94
commit
e595dbc88e
@ -304,7 +304,7 @@ function setup_all_libs ()
|
||||
-- names of external libraries used (see libraries_dir comment)
|
||||
local extern_libs = {}
|
||||
|
||||
|
||||
|
||||
source_dirs = {
|
||||
"network",
|
||||
}
|
||||
@ -326,7 +326,8 @@ function setup_all_libs ()
|
||||
"sound",
|
||||
"scripting",
|
||||
"maths",
|
||||
"maths/scripting"
|
||||
"maths/scripting",
|
||||
"dcdt/se"
|
||||
}
|
||||
extern_libs = {
|
||||
"spidermonkey",
|
||||
@ -343,12 +344,14 @@ function setup_all_libs ()
|
||||
"graphics",
|
||||
"graphics/scripting",
|
||||
"renderer"
|
||||
|
||||
}
|
||||
extern_libs = {
|
||||
"opengl",
|
||||
"sdl", -- key definitions
|
||||
"spidermonkey", -- for graphics/scripting
|
||||
"boost"
|
||||
|
||||
}
|
||||
setup_static_lib_package("graphics", source_dirs, extern_libs, {})
|
||||
|
||||
@ -369,12 +372,14 @@ function setup_all_libs ()
|
||||
source_dirs = {
|
||||
"tools/atlas/GameInterface",
|
||||
"tools/atlas/GameInterface/Handlers"
|
||||
|
||||
}
|
||||
extern_libs = {
|
||||
"boost",
|
||||
"sdl", -- key definitions
|
||||
"opengl",
|
||||
"spidermonkey"
|
||||
|
||||
}
|
||||
setup_static_lib_package("atlas", source_dirs, extern_libs, {})
|
||||
|
||||
|
||||
722
source/dcdt/se/Abstract.cpp
Normal file
722
source/dcdt/se/Abstract.cpp
Normal file
@ -0,0 +1,722 @@
|
||||
//Abstract.cpp
|
||||
|
||||
//DJD: Abstract function definitions {
|
||||
#include "precompiled.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <fstream>
|
||||
|
||||
#include "se_dcdt.h"
|
||||
|
||||
//the degree for an unabstracted triangle
|
||||
#define UNABSTRACTED -1
|
||||
|
||||
template <class T>
|
||||
SrArray<SeLinkFace<T> *> SeLinkFace<T>::processing;
|
||||
|
||||
void SeDcdt::Abstract()
|
||||
{
|
||||
#if defined EXPERIMENT
|
||||
//count the number of degree-3 nodes in the abstract graph
|
||||
int num = 0;
|
||||
#endif
|
||||
//Degree-1 and Degree-0 faces
|
||||
//keep track of the connected component
|
||||
int component = 0;
|
||||
//possible degree-1 triangles
|
||||
SrArray<SeDcdtFace *> degree1;
|
||||
//possible degree-2 or 3 triangles
|
||||
SrArray<SeDcdtFace *> processing;
|
||||
//get first face and record it
|
||||
SeDcdtFace *currentFace = _mesh->first()->fac();
|
||||
SeDcdtFace *firstFace = currentFace;
|
||||
if (outside == NULL)
|
||||
{
|
||||
//first determine the "outside" face
|
||||
// for (int n = 0; n < SeDcdtFace::Faces(); n++)
|
||||
do
|
||||
{
|
||||
// SeDcdtFace *currentFace = SeDcdtFace::Face(n);
|
||||
if (currentFace == NULL)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(currentFace, x1, y1, x2, y2, x3, y3);
|
||||
//outside face is any whose vertices aren't in counterclockwise order
|
||||
if (Orientation(x1, y1, x2, y2, x3, y3) <= 0.0f)
|
||||
{
|
||||
//record this face, reset, and break
|
||||
outside = currentFace;
|
||||
currentFace = firstFace;
|
||||
break;
|
||||
}
|
||||
//if it wasn't found, try the next face
|
||||
currentFace = currentFace->nxt();
|
||||
}
|
||||
while (currentFace != firstFace);
|
||||
}
|
||||
//go through all faces in the triangulation
|
||||
// for (int n = 0; n < SeDcdtFace::Faces(); n++)
|
||||
do
|
||||
{
|
||||
// SeDcdtFace *currentFace = SeDcdtFace::Face(n);
|
||||
//don't process the outside face
|
||||
if ((currentFace == NULL) || (currentFace == outside))
|
||||
{
|
||||
currentFace = currentFace->nxt();
|
||||
continue;
|
||||
}
|
||||
//keep track of how many edges are constrained
|
||||
int numConstrained = 0;
|
||||
//this is the adjacent face to a degree-1 triangle
|
||||
SeDcdtFace *tempFace = NULL;
|
||||
//access the parts of the face
|
||||
SeBase *s = currentFace->se();
|
||||
//count the number of constrained edges around this face
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//consider a constrained edge or one bordering the outside face to be blocked
|
||||
if (Blocked(s))
|
||||
{
|
||||
numConstrained++;
|
||||
}
|
||||
//if there's an unconstrained edge, record the face across it
|
||||
else
|
||||
{
|
||||
tempFace = (SeDcdtFace *)(s->sym()->fac());
|
||||
}
|
||||
//go to the next element of the triangle
|
||||
s = s->nxt();
|
||||
}
|
||||
//if there are two constrained edges, this is a degree-1 triangle
|
||||
if (numConstrained == 2)
|
||||
{
|
||||
//abstract the face and calculate its widths
|
||||
currentFace->link = new Abstraction(1);
|
||||
CalculateWidths(currentFace);
|
||||
//if the face across the unconstrained edge is already degree-1,
|
||||
if (Degree(tempFace) == 1)
|
||||
{
|
||||
//this is a tree component
|
||||
TreeAbstract(tempFace, component++);
|
||||
}
|
||||
//if it's not abstracted, it might now be a degree-1 node
|
||||
else if (Degree(tempFace) == UNABSTRACTED)
|
||||
{
|
||||
degree1.push() = tempFace;
|
||||
}
|
||||
}
|
||||
//if all edges are constrained, this is a degree-0 triangle
|
||||
else if (numConstrained == 3)
|
||||
{
|
||||
//abstract it and set its component
|
||||
currentFace->link = new Abstraction(0);
|
||||
currentFace->link->Component(component++);
|
||||
}
|
||||
//if it's not degree-0 or 1, it might be degree-2 or 3
|
||||
else
|
||||
{
|
||||
//enqueue it for later processing
|
||||
processing.push() = currentFace;
|
||||
}
|
||||
//move to the next face in the mesh
|
||||
currentFace = currentFace->nxt();
|
||||
}
|
||||
//continue until reaching the first face again
|
||||
while (currentFace != firstFace);
|
||||
//keep track of which faces we have considered for degree-1 nodes
|
||||
_mesh->begin_marking();
|
||||
//go through the possible degree-1 nodes
|
||||
while (!degree1.empty())
|
||||
{
|
||||
//get the next possible degree-1 node
|
||||
SeDcdtFace *currentFace = degree1.pop();
|
||||
//if this face has already been dealt with, skip it
|
||||
if ((_mesh->marked(currentFace)) || (Degree(currentFace) != UNABSTRACTED))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//keep track of the number of adjacent triangles
|
||||
int numAdjacent = 0;
|
||||
//the face across an unconstrained edge
|
||||
SeDcdtFace *tempFace = NULL;
|
||||
//access the elements of the triangle
|
||||
SeBase *s = currentFace->se();
|
||||
//calculate the number of unabstracted faces across unconstrained edges
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//only consider adjacent, unabstracted edges
|
||||
if (!Blocked(s) && (Degree(s->sym()) == UNABSTRACTED))
|
||||
{
|
||||
numAdjacent++;
|
||||
tempFace = (SeDcdtFace *)(s->sym()->fac());
|
||||
}
|
||||
//move to the next edge of the triangle
|
||||
s = s->nxt();
|
||||
}
|
||||
//if there is only one, this is a degree-1 node
|
||||
if (numAdjacent == 1)
|
||||
{
|
||||
//abstract this node and calculate its widths
|
||||
currentFace->link = new Abstraction(1);
|
||||
CalculateWidths(currentFace);
|
||||
//the adjacent triangle is now possibly degree-1
|
||||
degree1.push() = tempFace;
|
||||
//mark this face as dealt with
|
||||
_mesh->mark(currentFace);
|
||||
}
|
||||
//if there aren't any, this is a tree component
|
||||
else if (numAdjacent == 0)
|
||||
{
|
||||
TreeAbstract(currentFace, component++);
|
||||
//mark this face as dealt with
|
||||
_mesh->mark(currentFace);
|
||||
}
|
||||
}
|
||||
//done with degree-1 nodes
|
||||
_mesh->end_marking();
|
||||
|
||||
//Degree-2 and Degree-3 faces
|
||||
//go through all possible degree-3 triangles
|
||||
for (int n = 0; n < processing.size(); n++)
|
||||
{
|
||||
//gets the current face to check
|
||||
SeDcdtFace *currentFace = processing[n];
|
||||
//we only care about unabstracted nodes at this point
|
||||
if (Degree(currentFace) != UNABSTRACTED)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//the number of non-degree-1 triangles across unconstrained edges
|
||||
int numAdjacent = 0;
|
||||
//access the elements of the triangle
|
||||
SeBase *s = currentFace->se();
|
||||
//go through the edges of the triangle
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//get the degree of the triangle across that edge
|
||||
int degree = Degree(s->sym());
|
||||
//keep track of non-degree-1 edges across unconstrained edges
|
||||
if (!Blocked(s) && (degree != 1))
|
||||
{
|
||||
numAdjacent++;
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//if this is not a degree-3 triangle, skip it
|
||||
if (numAdjacent < 3)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//stack of degree-3 nodes in this connected component
|
||||
SrArray<SeDcdtFace *> degree3;
|
||||
//put the current triangle on the stack
|
||||
degree3.push() = currentFace;
|
||||
//continue through all degree-3 nodes in this component
|
||||
while (!degree3.empty())
|
||||
{
|
||||
//get one of the degree-3 nodes from the stack
|
||||
SeDcdtFace *stackFace = degree3.pop();
|
||||
//abstract it and calculate its widths and set its connected component
|
||||
stackFace->link = new Abstraction(3);
|
||||
#if defined EXPERIMENT
|
||||
//keep track of the extra degree-3 node
|
||||
num++;
|
||||
#endif
|
||||
CalculateWidths(stackFace);
|
||||
stackFace->link->Component(component);
|
||||
//access the elements of the triangle
|
||||
s = stackFace->se();
|
||||
//go in all directions from this node
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//get the face across this particular edge
|
||||
SeDcdtFace *tempFace = (SeDcdtFace *)(s->sym()->fac());
|
||||
//mark the first face and the previous face
|
||||
SeDcdtFace *firstFace = stackFace;
|
||||
SeDcdtFace *lastFace = stackFace;
|
||||
//keep track of the cumulative angle since the last degree-3 node
|
||||
float angle = 0;
|
||||
//follow this chain until another degree-3 node is encountered
|
||||
while (true)
|
||||
{
|
||||
//the number of adjacent faces across unconstrained edges not abstracted as degree-1
|
||||
numAdjacent = 0;
|
||||
//access the elements of this triangle
|
||||
SeBase *s1 = tempFace->se();
|
||||
//go through the edges of this triangle
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//get the degree of the face across this edge
|
||||
int degree = Degree(s1->sym());
|
||||
//if it's not blocked or degree-1, it's adjacent
|
||||
if (!Blocked(s1) && (degree != 1))
|
||||
{
|
||||
numAdjacent++;
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
//if the current triangle is degree-3,
|
||||
if (numAdjacent == 3)
|
||||
{
|
||||
//if it hasn't been abstracted yet, put it on the stack to deal with
|
||||
if (Degree(tempFace) == UNABSTRACTED)
|
||||
{
|
||||
degree3.push() = tempFace;
|
||||
}
|
||||
//sets the original degree-3 node adjacent to this one
|
||||
stackFace->link->Adjacent(i, tempFace);
|
||||
//and sets the total angle between them
|
||||
stackFace->link->Angle(i, angle);
|
||||
//if there were no degree-2 nodes in between these,
|
||||
if (lastFace == stackFace)
|
||||
{
|
||||
//get the elements of this triangle
|
||||
SeBase *s1 = tempFace->se();
|
||||
//go through the edges
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//find the edge between this triangle and the last one
|
||||
if (s1->sym()->fac() == lastFace)
|
||||
{
|
||||
//the choke point is the length of the edge between these faces
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(s1, x1, y1, x2, y2, x3, y3);
|
||||
stackFace->link->Choke(i, Length(x1, y1, x2, y2));
|
||||
break;
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
}
|
||||
//if there were degree-2 nodes between them,
|
||||
else
|
||||
{
|
||||
//access the elements of the previous triangle
|
||||
SeBase *s1 = lastFace->se();
|
||||
float choke, width;
|
||||
//go through its edges
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//find the direction of the original degree-3 node
|
||||
if (lastFace->link->Adjacent(j) == stackFace)
|
||||
{
|
||||
//get the choke point to this node
|
||||
choke = lastFace->link->Choke(j);
|
||||
//get the width through this triangle between the two degree-3 nodes
|
||||
width = lastFace->link->Width((s1->nxt()->sym()->fac() == tempFace) ? j : (j + 2) % 3);
|
||||
//the choke point of the original degree-3 node is the lesser of the two
|
||||
stackFace->link->Choke(i, Minimum(choke, width));
|
||||
break;
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
}
|
||||
//we've reached the other degree-3 node - stop following this chain
|
||||
break;
|
||||
}
|
||||
//if the triangle is degree-2
|
||||
else if (numAdjacent == 2)
|
||||
{
|
||||
//if the triangle wasn't abstracted before,
|
||||
if (Degree(tempFace) == UNABSTRACTED)
|
||||
{
|
||||
//abstract it and set its widths and connected component
|
||||
tempFace->link = new Abstraction(2);
|
||||
CalculateWidths(tempFace);
|
||||
tempFace->link->Component(component);
|
||||
}
|
||||
//access the elements of this triangle
|
||||
SeBase *s1 = tempFace->se();
|
||||
//go through the edges of the triangle
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//find the edge across which is the last triangle
|
||||
if (s1->sym()->fac() == lastFace)
|
||||
{
|
||||
//set the angle back towards the original degree-3 node
|
||||
tempFace->link->Angle(j, angle);
|
||||
//and set the adjacent degree-3 node in that direction
|
||||
tempFace->link->Adjacent(j, firstFace);
|
||||
//if there are no nodes between this one and the degree-3 node,
|
||||
if (lastFace == firstFace)
|
||||
{
|
||||
//the choke point is simply the length of the edge between them
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(s1, x1, y1, x2, y2, x3, y3);
|
||||
tempFace->link->Choke(j, Length(x1, y1, x2, y2));
|
||||
}
|
||||
else
|
||||
{
|
||||
float choke, width;
|
||||
//otherwise, access the elements of the last triangle
|
||||
SeBase *s2 = lastFace->se();
|
||||
//and go through the edges
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
//we want the edge which leads back to the original degree-3 node
|
||||
if ((s2->sym()->fac() == tempFace) || (lastFace->link->Adjacent(k) == NULL))
|
||||
{
|
||||
s2 = s2->nxt();
|
||||
continue;
|
||||
}
|
||||
//get the associated choke point width
|
||||
choke = lastFace->link->Choke(k);
|
||||
//get the width between the two degree-3 nodes
|
||||
if (s2->nxt()->sym()->fac() == tempFace)
|
||||
{
|
||||
width = lastFace->link->Width(k);
|
||||
}
|
||||
else
|
||||
{
|
||||
width = lastFace->link->Width((k + 2) % 3);
|
||||
}
|
||||
break;
|
||||
}
|
||||
//the choke point value is the lesser of these
|
||||
tempFace->link->Choke(j, Minimum(choke, width));
|
||||
}
|
||||
break;
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
//calculate the angle between edges leading to degree-3 nodes
|
||||
float currentAngle = 0;
|
||||
//access the elements of this triangle again
|
||||
s1 = tempFace->se();
|
||||
//record the next triangle to move to
|
||||
SeDcdtFace *nextFace = NULL;
|
||||
//go through the edges of the triangle
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//get the degree of the adjacent triangle across this edge
|
||||
int degree = Degree(s1->sym());
|
||||
//if there is a tree component off this node, collapse it
|
||||
if (!Blocked(s1) && (degree == 1)
|
||||
&& (((SeDcdtFace *)s1->sym()->fac())->link->Component() == INVALID))
|
||||
{
|
||||
TreeCollapse(tempFace, (SeDcdtFace *)(s1->sym()->fac()), component);
|
||||
}
|
||||
//when finding the next face, record it
|
||||
if (!Blocked(s1) && (s1->sym()->fac() != lastFace) && (degree != 1))
|
||||
{
|
||||
nextFace = (SeDcdtFace *)s1->sym()->fac();
|
||||
}
|
||||
//determine the degree of the triangle across the next edge
|
||||
int degree2 = Degree(s1->nxt()->sym());
|
||||
//if this edge and the next are the connecting edges, record the angle between them
|
||||
if (!((degree == 0) || (degree == 1) || (degree2 == 0) || (degree2 == 1))
|
||||
&& !Blocked(s1) && !Blocked(s1->nxt()))
|
||||
{
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(s1, x1, y1, x2, y2, x3, y3);
|
||||
currentAngle = abs(AngleBetween(x1, y1, x2, y2, x3, y3));
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
//increment the cumulative angle so far
|
||||
angle += currentAngle;
|
||||
//move to the next edge
|
||||
lastFace = tempFace;
|
||||
tempFace = nextFace;
|
||||
}
|
||||
//in other cases, report an error
|
||||
else
|
||||
{
|
||||
sr_out.warning("ERROR: should only encounter degree-2 and degree-3 faces this way\n");
|
||||
}
|
||||
}
|
||||
//follow another chain
|
||||
s = s->nxt();
|
||||
}
|
||||
}
|
||||
//get the next component
|
||||
component++;
|
||||
}
|
||||
|
||||
//Degree-2 faces (in a ring)
|
||||
//go through the processing queue again
|
||||
for (int i = 0; i < processing.size(); i++)
|
||||
{
|
||||
//get the current triangle
|
||||
SeDcdtFace *currentFace = processing[i];
|
||||
//only continue if it hasn't been abstracted yet
|
||||
if (Degree(currentFace) != UNABSTRACTED)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//mark it as the first triangle in the ring
|
||||
SeDcdtFace *firstFace = currentFace;
|
||||
//the next triangle to visit
|
||||
SeDcdtFace *nextFace = NULL;
|
||||
//follow the ring of unabstracted faces
|
||||
while (true)
|
||||
{
|
||||
//access the elements of the current triangle
|
||||
SeBase *s = currentFace->se();
|
||||
//go through its edges
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//record the degree of the triangle across this edge
|
||||
int degree = Degree(s->sym());
|
||||
//if there's a tree component off this face, collapse it
|
||||
if (!Blocked(s) && (degree == 1))
|
||||
{
|
||||
TreeCollapse(currentFace, (SeDcdtFace *)s->sym()->fac(), component);
|
||||
}
|
||||
//when the next face is found in the ring, record it
|
||||
if (!Blocked(s) && (degree == UNABSTRACTED))
|
||||
{
|
||||
nextFace = (SeDcdtFace *)s->sym()->fac();
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//abstract the current face, calculate its widths, and set its connected component
|
||||
currentFace->link = new Abstraction(2);
|
||||
CalculateWidths(currentFace);
|
||||
currentFace->link->Component(component);
|
||||
//access the triangle's elements again
|
||||
s = currentFace->se();
|
||||
//go through the edges again
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//if the adjacent triangle is part of the ring
|
||||
if (!Blocked(s) && Degree(s->sym()) != 1)
|
||||
{
|
||||
//set the angle and choke values accordingly
|
||||
currentFace->link->Angle(i, INFINITY);
|
||||
currentFace->link->Choke(i, INFINITY);
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//if the start of the ring has been reencountered, stop following it
|
||||
if (currentFace == nextFace)
|
||||
{
|
||||
break;
|
||||
}
|
||||
//move to the next face in the ring
|
||||
currentFace = nextFace;
|
||||
}
|
||||
//move to the next ring (connected component)
|
||||
component++;
|
||||
}
|
||||
#if defined EXPERIMENT
|
||||
//output the number of degree-3 nodes to the output file
|
||||
std::ofstream outFile("Data.txt", std::ios_base::app);
|
||||
outFile << "Degree3: " << num << "\t";
|
||||
outFile.close();
|
||||
#endif
|
||||
}
|
||||
|
||||
//Abstract into a tree
|
||||
void SeDcdt::TreeAbstract(SeDcdtFace *first, int component)
|
||||
{
|
||||
//create a stack of the nodes in the tree
|
||||
SrArray<SeDcdtFace *> tree;
|
||||
//put the initial node on the tree
|
||||
tree.push() = first;
|
||||
//continue until the tree can't be expanded anymore
|
||||
while (!tree.empty())
|
||||
{
|
||||
//get the next face off of the stack
|
||||
SeDcdtFace *currentFace = tree.pop();
|
||||
//get the elements of this triangle
|
||||
SeBase *s = currentFace->se();
|
||||
//go through its edges
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//ignore triangles across contrained edges
|
||||
if (Blocked(s))
|
||||
{
|
||||
s = s->nxt();
|
||||
continue;
|
||||
}
|
||||
//gets the face across that edge
|
||||
SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac();
|
||||
//if the adjacent triangle hasn't been dealt with, put it on the stack
|
||||
if ((tempFace->link->Angle(0) == INVALID) && (tempFace->link->Angle(1) == INVALID)
|
||||
&& (tempFace->link->Angle(2) == INVALID))
|
||||
{
|
||||
tree.push() = tempFace;
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//abstract the triangle if it hasn't already been
|
||||
if (Degree(currentFace) == UNABSTRACTED)
|
||||
{
|
||||
//if the triangle hasn't been abstracted yet, abstract it and calculate its widths
|
||||
currentFace->link = new Abstraction(1);
|
||||
CalculateWidths(currentFace);
|
||||
}
|
||||
//access the triangle's elements again
|
||||
s = currentFace->se();
|
||||
//move through its edges
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//for each unconstrained edge,
|
||||
if (!Blocked(s))
|
||||
{
|
||||
//set the angle and choke values accordingly
|
||||
currentFace->link->Angle(i, INFINITY);
|
||||
currentFace->link->Choke(i, INFINITY);
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//set the triangle's connected component
|
||||
currentFace->link->Component(component);
|
||||
}
|
||||
}
|
||||
|
||||
//Collapse a tree onto a degree-2 node
|
||||
void SeDcdt::TreeCollapse(SeDcdtFace *root, SeDcdtFace *currentFace, int component)
|
||||
{
|
||||
//mark edges we have dealt with
|
||||
_mesh->begin_marking();
|
||||
//starting with the root of the tree
|
||||
_mesh->mark(root);
|
||||
//create a stack of triangles to be processed
|
||||
SrArray<SeDcdtFace *> tree;
|
||||
//and a parallel one for angles of those triangles
|
||||
SrArray<float> angles;
|
||||
//put the current triangle on the stack to be processed
|
||||
tree.push() = currentFace;
|
||||
//and record that it is at angle 0 from the root
|
||||
angles.push() = 0.0f;
|
||||
//continue until all faces in the tree have been dealt with
|
||||
while (!tree.empty())
|
||||
{
|
||||
//get the top face off the stack
|
||||
currentFace = tree.pop();
|
||||
//set its connected component
|
||||
currentFace->link->Component(component);
|
||||
//mark this face as dealt with
|
||||
_mesh->mark(currentFace);
|
||||
//access this face's elements
|
||||
SeBase *s = currentFace->se();
|
||||
//the angles to the other adjacent triangles
|
||||
float angle[2];
|
||||
|
||||
int i;
|
||||
//go through the edges
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
//the face across that edge
|
||||
SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac();
|
||||
//if this is the entry edge,
|
||||
if (!Blocked(s) && _mesh->marked(tempFace))
|
||||
{
|
||||
//set the angle
|
||||
currentFace->link->Angle(i, angles.pop());
|
||||
//calculate the angle to the other adjacent triangles
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(s, x1, y1, x2, y2, x3, y3);
|
||||
angle[0] = abs(AngleBetween(x1, y1, x2, y2, x3, y3)) + currentFace->link->Angle(i);
|
||||
angle[1] = abs(AngleBetween(x2, y2, x3, y3, x1, y1)) + currentFace->link->Angle(i);
|
||||
//set the adjacent triangle in that direction to the root
|
||||
currentFace->link->Adjacent(i, root);
|
||||
//if this is the triangle next to the root,
|
||||
if (tempFace == root)
|
||||
{
|
||||
//the choke value is the length of the edge between them
|
||||
currentFace->link->Choke(i, Length(x1, y1, x2, y2));
|
||||
}
|
||||
//if there are triangles between this and the root,
|
||||
else
|
||||
{
|
||||
//get the elements of the current triangle
|
||||
SeBase *s1 = tempFace->se();
|
||||
float width, choke;
|
||||
//go through its edges
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
//get the choke value in that direction
|
||||
float temp = tempFace->link->Choke(j);
|
||||
//if it's a valid value,
|
||||
if ((temp != INVALID) && (temp != INFINITY))
|
||||
{
|
||||
//record it
|
||||
choke = temp;
|
||||
//gets the width through that triangle between the root and current triangles
|
||||
if (s1->nxt()->sym()->fac() == currentFace)
|
||||
{
|
||||
width = tempFace->link->Width(j);
|
||||
}
|
||||
else
|
||||
{
|
||||
width = tempFace->link->Width((j + 2) % 3);
|
||||
}
|
||||
break;
|
||||
}
|
||||
//move to the next edge
|
||||
s1 = s1->nxt();
|
||||
}
|
||||
//the choke value of this triangle is the lesser of these values
|
||||
currentFace->link->Choke(i, Minimum(width, choke));
|
||||
}
|
||||
break;
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//record where we left off
|
||||
int base = i + 1;
|
||||
//visit the other two adjacent triangles
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
//only deal with ones not across a constrained edge
|
||||
if (Blocked(s))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
//set the angle and choke values acoordingly
|
||||
currentFace->link->Angle((i + base) % 3, INFINITY);
|
||||
currentFace->link->Choke((i + base) % 3, INFINITY);
|
||||
//put them on the tree for processing
|
||||
tree.push() = (SeDcdtFace *)s->sym()->fac();
|
||||
//also put the appropriate angle on the other tree
|
||||
angles.push() = angle[i];
|
||||
}
|
||||
}
|
||||
//finished collapsing the tree
|
||||
_mesh->end_marking();
|
||||
}
|
||||
|
||||
//deletes the information from the abstraction
|
||||
void SeDcdt::DeleteAbstraction()
|
||||
{
|
||||
//get the first face in the mesh
|
||||
SeDcdtFace *currentFace = _mesh->first()->fac();
|
||||
//and record it
|
||||
SeDcdtFace *firstFace = currentFace;
|
||||
//go through the faces
|
||||
do
|
||||
{
|
||||
//if the triangle is abstracted,
|
||||
if (currentFace->link != NULL)
|
||||
{
|
||||
//free the abstraction information
|
||||
delete currentFace->link;
|
||||
currentFace->link = NULL;
|
||||
}
|
||||
//move to the next triangle
|
||||
currentFace = currentFace->nxt();
|
||||
}
|
||||
//until reaching the first face again
|
||||
while (currentFace != firstFace);
|
||||
//mark the outside face as being invalid
|
||||
outside = NULL;
|
||||
}
|
||||
|
||||
//Abstract function definitions }
|
||||
157
source/dcdt/se/Abstraction.h
Normal file
157
source/dcdt/se/Abstraction.h
Normal file
@ -0,0 +1,157 @@
|
||||
//Abstraction.h
|
||||
|
||||
//DJD: definition of Abstraction class {
|
||||
|
||||
#ifndef ABSTRACTION_H
|
||||
#define ABSTRACTION_H
|
||||
|
||||
#include <float.h>
|
||||
#include "FunnelDeque.h"
|
||||
|
||||
//defines an invalid entry, and "infinity", for our purposes
|
||||
#define INVALID -1
|
||||
#define INFINITY FLT_MAX
|
||||
|
||||
//prototypes for classes defined later
|
||||
template <class T> class SeLinkFace;
|
||||
class Abstraction;
|
||||
typedef SeLinkFace<Abstraction> SeDcdtFace;
|
||||
|
||||
//class which contains information about our topological abstraction
|
||||
class Abstraction
|
||||
{
|
||||
protected:
|
||||
//list of adjacent nodes
|
||||
//if this node is degree-1 in a tree, all will be NULL
|
||||
//if it is degree-1 otherwise, one will be the root of the tree, the others will be NULL
|
||||
//if it is degree-2 in a ring, all will be NULL
|
||||
//if it is degree-2 otherwise, two will be degree-3 nodes on the ends of the corridor, the other will be NULL
|
||||
//if it is degree-3, all will be degree-3 nodes on the end of the corridors
|
||||
SeDcdtFace *m_CAdjacent[3];
|
||||
//sum of interior angles of triangles between this one and the corresponding adjacent one
|
||||
//INVALID if the corresponding adjacent node is NULL
|
||||
float m_dAngle[3];
|
||||
//width between the corresponding edge and the one counterclockwise to it, through the triangle
|
||||
float m_dWidth[3];
|
||||
//smallest width through the triangles between this one and the corresponding adjacent one
|
||||
//INVALID if the corresponding adjacent node is NULL
|
||||
float m_dChoke[3];
|
||||
//degree of this node
|
||||
int m_nDegree;
|
||||
//connected component to which this triangle belongs
|
||||
int m_nComponent;
|
||||
|
||||
//makes sure the index is in the range [0, 2]
|
||||
bool CheckBounds(int n)
|
||||
{
|
||||
return ((n >= 0) && (n < 3));
|
||||
}
|
||||
public:
|
||||
//creates the abstraction with given degree
|
||||
Abstraction(int nDegree)
|
||||
{
|
||||
//starts all adjacents as NULL and other information as INVALID
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
m_CAdjacent[i] = NULL;
|
||||
m_dAngle[i] = INVALID;
|
||||
m_dWidth[i] = INVALID;
|
||||
m_dChoke[i] = INVALID;
|
||||
}
|
||||
m_nDegree = nDegree;
|
||||
m_nComponent = INVALID;
|
||||
}
|
||||
|
||||
//default destructor
|
||||
~Abstraction()
|
||||
{
|
||||
}
|
||||
|
||||
//accessor for the degree of the node
|
||||
int Degree()
|
||||
{
|
||||
return m_nDegree;
|
||||
}
|
||||
|
||||
//mutator for the degree of the node
|
||||
void Degree(int nDegree)
|
||||
{
|
||||
m_nDegree = nDegree;
|
||||
}
|
||||
|
||||
//accessor for the connected component of the node
|
||||
int Component()
|
||||
{
|
||||
return m_nComponent;
|
||||
}
|
||||
|
||||
//mutator for the connected component of the node
|
||||
void Component(int nComponent)
|
||||
{
|
||||
m_nComponent = nComponent;
|
||||
}
|
||||
|
||||
//accessor for the adjacent node with index given
|
||||
SeDcdtFace *Adjacent(int n)
|
||||
{
|
||||
return (CheckBounds(n)) ? m_CAdjacent[n] : NULL;
|
||||
}
|
||||
|
||||
//mutator for the adjacent node with index given
|
||||
void Adjacent(int n, SeDcdtFace *CAdjacent)
|
||||
{
|
||||
if (CheckBounds(n))
|
||||
{
|
||||
m_CAdjacent[n] = CAdjacent;
|
||||
}
|
||||
}
|
||||
|
||||
//accessor for the angle with index given
|
||||
float Angle(int n)
|
||||
{
|
||||
return (CheckBounds(n)) ? m_dAngle[n] : INVALID;
|
||||
}
|
||||
|
||||
//mutator for the angle with index given
|
||||
void Angle(int n, float dAngle)
|
||||
{
|
||||
if (CheckBounds(n))
|
||||
{
|
||||
m_dAngle[n] = dAngle;
|
||||
}
|
||||
}
|
||||
|
||||
//accessor for the width with index given
|
||||
float Width(int n)
|
||||
{
|
||||
return (CheckBounds(n)) ? m_dWidth[n] : INVALID;
|
||||
}
|
||||
|
||||
//mutator for the width with index given
|
||||
void Width(int n, float dWidth)
|
||||
{
|
||||
if (CheckBounds(n))
|
||||
{
|
||||
m_dWidth[n] = dWidth;
|
||||
}
|
||||
}
|
||||
|
||||
//accessor for the choke point width with index given
|
||||
float Choke(int n)
|
||||
{
|
||||
return (CheckBounds(n)) ? m_dChoke[n] : INVALID;
|
||||
}
|
||||
|
||||
//mutator for the choke point width with index given
|
||||
void Choke(int n, float dChoke)
|
||||
{
|
||||
if (CheckBounds(n))
|
||||
{
|
||||
m_dChoke[n] = dChoke;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
//definition of Abstraction class }
|
||||
100
source/dcdt/se/Experiments.h
Normal file
100
source/dcdt/se/Experiments.h
Normal file
@ -0,0 +1,100 @@
|
||||
//Experiments.h
|
||||
|
||||
//DJD: definitions used in running experiments {
|
||||
|
||||
#ifndef EXPERIMENTS_H
|
||||
#define EXPERIMENTS_H
|
||||
|
||||
//windows-dependent timing mechanism
|
||||
#include <windows.h>
|
||||
|
||||
//for accurate timing of paths
|
||||
class Timer
|
||||
{
|
||||
protected:
|
||||
//time at which the timer was started
|
||||
LARGE_INTEGER StartTime;
|
||||
// LARGE_INTEGER StopTime;
|
||||
//frequency of the counter
|
||||
LONGLONG Frequency;
|
||||
//calibration component - time between lines
|
||||
LONGLONG Correction;
|
||||
public:
|
||||
//constructor
|
||||
Timer()
|
||||
{
|
||||
//retrieves the frequency of the counter
|
||||
LARGE_INTEGER freq;
|
||||
QueryPerformanceFrequency(&freq);
|
||||
//gets the relevant portion of that data
|
||||
Frequency = freq.QuadPart;
|
||||
LARGE_INTEGER StopTime;
|
||||
//starts the timer
|
||||
Start();
|
||||
// Stop();
|
||||
//and measures the time between these lines
|
||||
QueryPerformanceCounter(&StopTime);
|
||||
//stores the relevant part of that figure
|
||||
Correction = StopTime.QuadPart - StartTime.QuadPart;
|
||||
}
|
||||
|
||||
//start timing
|
||||
void Start()
|
||||
{
|
||||
// Sleep(0);
|
||||
//record the start time
|
||||
QueryPerformanceCounter(&StartTime);
|
||||
}
|
||||
|
||||
// void Stop()
|
||||
// {
|
||||
// QueryPerformanceCounter(&StopTime);
|
||||
// }
|
||||
|
||||
//return the time since the start time in milliseconds
|
||||
float GetDuration()
|
||||
{
|
||||
//get the current time from the timer
|
||||
LARGE_INTEGER StopTime;
|
||||
QueryPerformanceCounter(&StopTime);
|
||||
//return the calculated duration since the timer was started
|
||||
return (float)(StopTime.QuadPart - StartTime.QuadPart - Correction) * 1000.0f / Frequency;
|
||||
}
|
||||
};
|
||||
|
||||
//structure for holding data from the experiments
|
||||
struct Data
|
||||
{
|
||||
//the total time the algorithm ran
|
||||
float TotalTime;
|
||||
//the time taken to construct channels, run funnel algorithms, etc.
|
||||
float ConstructionTime;
|
||||
//the length of the best path found to this point
|
||||
float Length;
|
||||
//the number of nodes searched at this point
|
||||
int SearchNodes;
|
||||
//the number of triangles used in funnel algorithms, etc.
|
||||
int ConstructionNodes;
|
||||
//the number of paths found so far
|
||||
int Paths;
|
||||
//constructor
|
||||
Data()
|
||||
{
|
||||
//initializes all variables
|
||||
Reset();
|
||||
}
|
||||
//initializes all variables to default values
|
||||
void Reset()
|
||||
{
|
||||
TotalTime = 0.0f;
|
||||
ConstructionTime = 0.0f;
|
||||
Length = 0.0f;
|
||||
SearchNodes = 0;
|
||||
ConstructionNodes = 0;
|
||||
Paths = 1;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
//definitions used in running experiments }
|
||||
557
source/dcdt/se/FunnelDeque.cpp
Normal file
557
source/dcdt/se/FunnelDeque.cpp
Normal file
@ -0,0 +1,557 @@
|
||||
//FunnelDeque.cpp
|
||||
|
||||
//DJD: definition of FunnelNode functions {
|
||||
|
||||
#include "precompiled.h"
|
||||
|
||||
#include "FunnelDeque.h"
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
|
||||
//default constructor - initializes all
|
||||
//member variables to default values
|
||||
FunnelNode::FunnelNode()
|
||||
{
|
||||
m_CLeft = NULL;
|
||||
m_CRight = NULL;
|
||||
m_CPoint.set(FLT_MIN, FLT_MIN);
|
||||
}
|
||||
|
||||
//constructor - initializes the point member
|
||||
//variable to the coordinates passed
|
||||
FunnelNode::FunnelNode(float x, float y)
|
||||
{
|
||||
m_CLeft = NULL;
|
||||
m_CRight = NULL;
|
||||
m_CPoint.set(x, y);
|
||||
}
|
||||
|
||||
//constructor - initializes the point member
|
||||
//variable to the one passed
|
||||
FunnelNode::FunnelNode(const SrPnt2& point)
|
||||
{
|
||||
m_CLeft = NULL;
|
||||
m_CRight = NULL;
|
||||
m_CPoint.set(point.x, point.y);
|
||||
}
|
||||
|
||||
//mutator for the left pointer
|
||||
void FunnelNode::Left(FunnelNode *left)
|
||||
{
|
||||
m_CLeft = left;
|
||||
}
|
||||
|
||||
//mutator for the right pointer
|
||||
void FunnelNode::Right(FunnelNode *right)
|
||||
{
|
||||
m_CRight = right;
|
||||
}
|
||||
|
||||
//accessor for the left pointer
|
||||
FunnelNode *FunnelNode::Left()
|
||||
{
|
||||
return m_CLeft;
|
||||
}
|
||||
|
||||
//accessor for the right pointer
|
||||
FunnelNode *FunnelNode::Right()
|
||||
{
|
||||
return m_CRight;
|
||||
}
|
||||
|
||||
//mutator for the point
|
||||
void FunnelNode::Point(const SrPnt2& point)
|
||||
{
|
||||
m_CPoint.set(point.x, point.y);
|
||||
}
|
||||
|
||||
//accessor for the point
|
||||
SrPnt2 FunnelNode::Point()
|
||||
{
|
||||
SrPnt2 copy;
|
||||
copy.set(m_CPoint.x, m_CPoint.y);
|
||||
return copy;
|
||||
}
|
||||
|
||||
//mutator for the point's coordinates
|
||||
void FunnelNode::Point(float x, float y)
|
||||
{
|
||||
m_CPoint.set(x, y);
|
||||
}
|
||||
|
||||
//accessor for the point's X coordinate
|
||||
float FunnelNode::X()
|
||||
{
|
||||
return m_CPoint.x;
|
||||
}
|
||||
|
||||
//accessor for the point's Y coordinate
|
||||
float FunnelNode::Y()
|
||||
{
|
||||
return m_CPoint.y;
|
||||
}
|
||||
|
||||
//definition of FunnelNode functions }
|
||||
|
||||
//DJD: definition of FunnelDeque functions {
|
||||
|
||||
//constructor - initializes the apex of the funnel
|
||||
//to the point passed, and the unit radius
|
||||
FunnelDeque::FunnelDeque(float x, float y, float r)
|
||||
{
|
||||
m_CApex = new FunnelNode(x, y);
|
||||
m_CLeft = m_CApex;
|
||||
m_CRight = m_CApex;
|
||||
m_tApexType = Point;
|
||||
m_dRadius = r;
|
||||
}
|
||||
|
||||
//constructor - initializes the apex of the funnel
|
||||
//to the coordinates passed, and the unit radius
|
||||
FunnelDeque::FunnelDeque(const SrPnt2& point, float r)
|
||||
{
|
||||
m_CApex = new FunnelNode(point);
|
||||
m_CLeft = m_CApex;
|
||||
m_CRight = m_CApex;
|
||||
m_tApexType = Point;
|
||||
m_dRadius = r;
|
||||
}
|
||||
|
||||
//destructor - deletes any funnel nodes in the deque
|
||||
FunnelDeque::~FunnelDeque()
|
||||
{
|
||||
FunnelNode *current = m_CLeft;
|
||||
while (current != NULL)
|
||||
{
|
||||
FunnelNode *temp = current;
|
||||
current = current->Right();
|
||||
delete temp;
|
||||
}
|
||||
}
|
||||
|
||||
//Adds a new point to the funnel, adds to the existing path if the apex moves
|
||||
//type = RightTangent => point is being added on left side
|
||||
//type = LeftTangent => point is being added on right side
|
||||
//type = Point => point is the end of the channel
|
||||
void FunnelDeque::Add(const SrPnt2& p, CornerType type, SrPolygon& path)
|
||||
{
|
||||
//the new funnel node containing the new point
|
||||
FunnelNode *next = new FunnelNode(p);
|
||||
//if it's being added on the left side
|
||||
//(operations are just reversed for the other side)
|
||||
if (type == RightTangent)
|
||||
{
|
||||
//loop until the point is added to the funnel
|
||||
while (true)
|
||||
{
|
||||
//if the apex is the only point in the funnel,
|
||||
//simply add the point to the appropriate side
|
||||
if (m_CLeft == m_CRight)
|
||||
{
|
||||
next->Right(m_CApex);
|
||||
m_CApex->Left(next);
|
||||
m_CLeft = next;
|
||||
break;
|
||||
}
|
||||
float x_1, y_1, x_2, y_2;
|
||||
CornerType type_1, type_2;
|
||||
//if there are no points on the left side of the funnel,
|
||||
//get the wedge going to the opposite side of the apex
|
||||
if (m_CLeft == m_CApex)
|
||||
{
|
||||
x_1 = m_CLeft->X();
|
||||
y_1 = m_CLeft->Y();
|
||||
x_2 = m_CLeft->Right()->X();
|
||||
y_2 = m_CLeft->Right()->Y();
|
||||
type_1 = m_tApexType;
|
||||
type_2 = LeftTangent;
|
||||
}
|
||||
//otherwise get the wedge on this side of the apex
|
||||
else
|
||||
{
|
||||
x_2 = m_CLeft->X();
|
||||
y_2 = m_CLeft->Y();
|
||||
x_1 = m_CLeft->Right()->X();
|
||||
y_1 = m_CLeft->Right()->Y();
|
||||
type_2 = RightTangent;
|
||||
if (m_CLeft->Right() == m_CApex)
|
||||
{
|
||||
type_1 = m_tApexType;
|
||||
}
|
||||
else
|
||||
{
|
||||
type_1 = RightTangent;
|
||||
}
|
||||
}
|
||||
//calculate the angle associated with this wedge
|
||||
float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
|
||||
//also calculate the distance between these two points
|
||||
float length1 = Distance(x_1, y_1, x_2, y_2);
|
||||
//and that to the new point (special case for the modofied funnel algorithm)
|
||||
float length2 = Distance(x_1, y_1, p.x, p.y);
|
||||
//now get the wedge leading to the new point
|
||||
x_1 = m_CLeft->X();
|
||||
y_1 = m_CLeft->Y();
|
||||
if (m_CApex == m_CLeft)
|
||||
{
|
||||
type_1 = m_tApexType;
|
||||
}
|
||||
else
|
||||
{
|
||||
type_1 = RightTangent;
|
||||
}
|
||||
x_2 = p.x;
|
||||
y_2 = p.y;
|
||||
type_2 = type;
|
||||
//and the angle associated with it
|
||||
float toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
|
||||
//compare the two angles
|
||||
float diff = wedge - toPoint;
|
||||
diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f;
|
||||
//if the existing wedge is counterclockwise of that to the point
|
||||
if (diff < 0.0f)
|
||||
{
|
||||
//add the new point on this end of the funnel
|
||||
m_CLeft->Left(next);
|
||||
next->Right(m_CLeft);
|
||||
m_CLeft = next;
|
||||
break;
|
||||
}
|
||||
//if it is clockwise,
|
||||
else
|
||||
{
|
||||
//check if we are popping the apex
|
||||
if (m_CLeft == m_CApex)
|
||||
{
|
||||
//if the point being added is closer than the one being popped,
|
||||
if (length2 < length1)
|
||||
{
|
||||
//make the new point the apex instead of the one on the opposite side
|
||||
//(special case for the modified funnel algorithm)
|
||||
float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, p.x, p.y, type);
|
||||
AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path);
|
||||
AddPoint(p.x, p.y, type, angle, path);
|
||||
next->Right(m_CApex->Right());
|
||||
m_CApex->Right()->Left(next);
|
||||
delete m_CApex;
|
||||
m_CApex = next;
|
||||
m_CLeft = next;
|
||||
m_tApexType = type;
|
||||
break;
|
||||
}
|
||||
//otherwise,
|
||||
else
|
||||
{
|
||||
//move the apex to the right and add the segment between
|
||||
//the old and new apexes to the path so far
|
||||
float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType,
|
||||
m_CApex->Right()->X(), m_CApex->Right()->Y(), CornerType::LeftTangent);
|
||||
AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path);
|
||||
AddPoint(m_CApex->Right()->X(), m_CApex->Right()->Y(), CornerType::LeftTangent, angle, path);
|
||||
m_CApex = m_CApex->Right();
|
||||
m_tApexType = LeftTangent;
|
||||
}
|
||||
}
|
||||
//pop the leftmost point off of the funnel
|
||||
FunnelNode *temp = m_CLeft;
|
||||
m_CLeft = m_CLeft->Right();
|
||||
m_CLeft->Left(NULL);
|
||||
delete temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
//otherwise, if adding a point on the right side,
|
||||
//continue as before, with sides reversed
|
||||
//adding a point also ends up here -
|
||||
//we do this to pop off all the necessary points off of
|
||||
//the right side of the funnel, so at the end we can just
|
||||
//add to the path the points between the apex and the
|
||||
//right side of the funnel (done at the end)
|
||||
else
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
if (m_CLeft == m_CRight)
|
||||
{
|
||||
next->Left(m_CApex);
|
||||
m_CApex->Right(next);
|
||||
m_CRight = next;
|
||||
break;
|
||||
}
|
||||
float x_1, y_1, x_2, y_2;
|
||||
CornerType type_1, type_2;
|
||||
if (m_CRight == m_CApex)
|
||||
{
|
||||
x_1 = m_CRight->X();
|
||||
y_1 = m_CRight->Y();
|
||||
x_2 = m_CRight->Left()->X();
|
||||
y_2 = m_CRight->Left()->Y();
|
||||
type_1 = m_tApexType;
|
||||
type_2 = RightTangent;
|
||||
}
|
||||
else
|
||||
{
|
||||
x_2 = m_CRight->X();
|
||||
y_2 = m_CRight->Y();
|
||||
x_1 = m_CRight->Left()->X();
|
||||
y_1 = m_CRight->Left()->Y();
|
||||
type_2 = LeftTangent;
|
||||
if (m_CRight->Left() == m_CApex)
|
||||
{
|
||||
type_1 = m_tApexType;
|
||||
}
|
||||
else
|
||||
{
|
||||
type_1 = LeftTangent;
|
||||
}
|
||||
}
|
||||
float wedge = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
|
||||
float length1 = Distance(x_1, y_1, x_2, y_2);
|
||||
float length2 = Distance(x_1, y_1, p.x, p.y);
|
||||
x_1 = m_CRight->X();
|
||||
y_1 = m_CRight->Y();
|
||||
if (m_CApex == m_CRight)
|
||||
{
|
||||
type_1 = m_tApexType;
|
||||
}
|
||||
else
|
||||
{
|
||||
type_1 = LeftTangent;
|
||||
}
|
||||
x_2 = p.x;
|
||||
y_2 = p.y;
|
||||
type_2 = type;
|
||||
float toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
|
||||
float diff = wedge - toPoint;
|
||||
diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f;
|
||||
if (diff < 0.0f)
|
||||
{
|
||||
if (m_CRight == m_CApex)
|
||||
{
|
||||
if (length2 < length1)
|
||||
{
|
||||
float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType, p.x, p.y, type);
|
||||
AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path);
|
||||
AddPoint(p.x, p.y, type, angle, path);
|
||||
next->Left(m_CApex->Left());
|
||||
m_CApex->Left()->Right(next);
|
||||
delete m_CApex;
|
||||
m_CApex = next;
|
||||
m_CRight = next;
|
||||
m_tApexType = type;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
float angle = Angle(m_CApex->X(), m_CApex->Y(), m_tApexType,
|
||||
m_CApex->Left()->X(), m_CApex->Left()->Y(), CornerType::RightTangent);
|
||||
AddPoint(m_CApex->X(), m_CApex->Y(), m_tApexType, angle, path);
|
||||
AddPoint(m_CApex->Left()->X(), m_CApex->Left()->Y(), CornerType::RightTangent, angle, path);
|
||||
m_CApex = m_CApex->Left();
|
||||
m_tApexType = RightTangent;
|
||||
}
|
||||
}
|
||||
FunnelNode *temp = m_CRight;
|
||||
m_CRight = m_CRight->Left();
|
||||
m_CRight->Right(NULL);
|
||||
delete temp;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_CRight->Right(next);
|
||||
next->Left(m_CRight);
|
||||
m_CRight = next;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//this is where the points between the apex
|
||||
//and the right side o fthe funnel are added to the path
|
||||
//when adding the final point to the channel
|
||||
if (type == Point)
|
||||
{
|
||||
FunnelNode *Current = m_CApex;
|
||||
while (Current != m_CRight)
|
||||
{
|
||||
float x_1, y_1, x_2, y_2, toPoint;
|
||||
CornerType type_1, type_2;
|
||||
type_1 = (Current == m_CApex) ? m_tApexType : CornerType::LeftTangent;
|
||||
type_2 = (Current->Right() == m_CRight) ? CornerType::Point : CornerType::LeftTangent;
|
||||
x_1 = Current->X();
|
||||
y_1 = Current->Y();
|
||||
Current = Current->Right();
|
||||
x_2 = Current->X();
|
||||
y_2 = Current->Y();
|
||||
toPoint = Angle(x_1, y_1, type_1, x_2, y_2, type_2);
|
||||
AddPoint(x_1, y_1, type_1, toPoint, path);
|
||||
AddPoint(x_2, y_2, type_2, toPoint, path);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//returns the length of a path found by this funnel algorithm with the current radius
|
||||
float FunnelDeque::Length(SrPolygon path)
|
||||
{
|
||||
//only paths with an even number of points are valid
|
||||
if ((path.size() % 2) != 0)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
float length = 0.0f;
|
||||
//go through the straight segments and total their lengths
|
||||
for (int i = 0; i < path.size(); i += 2)
|
||||
{
|
||||
float xDiff = path[i + 1].x - path[i].x;
|
||||
float yDiff = path[i + 1].y - path[i].y;
|
||||
length += sqrt(xDiff * xDiff + yDiff * yDiff);
|
||||
}
|
||||
//go through the curved segments and add their length
|
||||
//(the difference of the angles of the
|
||||
// surrounding segments, times the unit radius)
|
||||
for (int i = 1; i < path.size() - 3; i += 2)
|
||||
{
|
||||
float angle1 = atan2(path[i + 1].y - path[i].y, path[i + 1].x - path[i].x);
|
||||
float angle2 = atan2(path[i + 3].y - path[i + 2].y, path[i + 3].x - path[i + 2].x);
|
||||
float diff = angle1 - angle2;
|
||||
diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f;
|
||||
length += m_dRadius * abs(diff);
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
//returns the angle from x_1, y_1 to x_2, y_2, given the types
|
||||
float FunnelDeque::Angle(float x_1, float y_1, CornerType type_1, float x_2, float y_2, CornerType type_2)
|
||||
{
|
||||
//simply calls the appropriate function based
|
||||
//on the types of the two points
|
||||
if (type_1 == Point)
|
||||
{
|
||||
return PointToTangent(x_1, y_1, x_2, y_2, (type_2 == RightTangent));
|
||||
}
|
||||
else if (type_2 == Point)
|
||||
{
|
||||
float angle = PointToTangent(x_2, y_2, x_1, y_1, (type_1 == LeftTangent));
|
||||
return AngleRange(angle + PI);
|
||||
}
|
||||
else if (type_1 != type_2)
|
||||
{
|
||||
return ToTangentAlt(x_1, y_1, x_2, y_2, (type_1 == LeftTangent));
|
||||
}
|
||||
else
|
||||
{
|
||||
return ToTangent(x_1, y_1, x_2, y_2);
|
||||
}
|
||||
}
|
||||
|
||||
//adds a point to the path given a coordinate, its type,
|
||||
//and the angle of the segment attached
|
||||
void FunnelDeque::AddPoint(float x, float y, CornerType type, float angle, SrPolygon &path)
|
||||
{
|
||||
//if it's a point type, just add it unchanged
|
||||
if (type == Point)
|
||||
{
|
||||
path.push().set(x, y);
|
||||
}
|
||||
//otherwise,
|
||||
else
|
||||
{
|
||||
//move the unit radius from the point to one side or the other
|
||||
float theta = AngleRange(angle + PI / ((type == LeftTangent) ? 2.0f : -2.0f));
|
||||
path.push().set(x + cos(theta) * m_dRadius, y + sin(theta) * m_dRadius);
|
||||
}
|
||||
}
|
||||
|
||||
//takes an angle value and restricts it to the range (-PI, PI]
|
||||
float FunnelDeque::AngleRange(float theta)
|
||||
{
|
||||
return ((theta > PI) ? (theta - 2.0f * PI) :
|
||||
(theta <= -PI) ? (theta + 2.0f * PI) : theta);
|
||||
}
|
||||
|
||||
//gets the distance between the points (x_1, y_1) and (x_2, y_2)
|
||||
float FunnelDeque::Distance(float x_1, float y_1, float x_2, float y_2)
|
||||
{
|
||||
float x = x_2 - x_1;
|
||||
float y = y_2 - y_1;
|
||||
return sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
//gets the angle of the line tangent to two points of the same type
|
||||
float FunnelDeque::ToTangent(float x_1, float y_1, float x_2, float y_2)
|
||||
{
|
||||
return atan2(y_2 - y_1, x_2 - x_1);
|
||||
}
|
||||
|
||||
//gets the angle of the line tangent to a point
|
||||
//of type LeftTangent and one of type RightTangent
|
||||
float FunnelDeque::ToTangentAlt(float x_1, float y_1, float x_2, float y_2, bool LeftRight)
|
||||
{
|
||||
float h = Distance(x_1, y_1, x_2, y_2) / 2.0f;
|
||||
float l = sqrt(h * h - m_dRadius * m_dRadius);
|
||||
float interior = atan(m_dRadius / l);
|
||||
float absolute = atan2(y_2 - y_1, x_2 - x_1);
|
||||
if (LeftRight)
|
||||
{
|
||||
interior = -interior;
|
||||
}
|
||||
return AngleRange(absolute + interior);
|
||||
}
|
||||
|
||||
//gets the angle of the line going through one point
|
||||
//and running tangent to another
|
||||
float FunnelDeque::PointToTangent(float x_1, float y_1, float x_2, float y_2, bool Right)
|
||||
{
|
||||
float h = Distance(x_1, y_1, x_2, y_2);
|
||||
float l = sqrt(h * h - m_dRadius * m_dRadius);
|
||||
float interior = atan(m_dRadius / l);
|
||||
float absolute = atan2(y_2 - y_1, x_2 - x_1);
|
||||
if (Right)
|
||||
{
|
||||
interior = -interior;
|
||||
}
|
||||
return AngleRange(absolute + interior);
|
||||
}
|
||||
|
||||
//prints the funnel (used for debugging)
|
||||
void FunnelDeque::Print()
|
||||
{
|
||||
FunnelNode *Current = m_CLeft;
|
||||
while (Current != NULL)
|
||||
{
|
||||
sr_out << "\t(" << Current->X() << ", " << Current->Y() << ")";
|
||||
if (Current == m_CLeft)
|
||||
{
|
||||
sr_out << " <-- Left";
|
||||
}
|
||||
if (Current == m_CApex)
|
||||
{
|
||||
sr_out << " <-- Apex";
|
||||
}
|
||||
if (Current == m_CRight)
|
||||
{
|
||||
sr_out << " <-- Right";
|
||||
}
|
||||
sr_out << srnl;
|
||||
Current = Current->Right();
|
||||
}
|
||||
}
|
||||
|
||||
//prints the path and then the funnel (used for debugging)
|
||||
void FunnelDeque::Print(SrPolygon path)
|
||||
{
|
||||
sr_out << "Path = {";
|
||||
for (int i = 0; i < path.size() - 1; i++)
|
||||
{
|
||||
SrPnt2 p = path[i];
|
||||
sr_out << "(" << p.x << ", " << p.y << "), ";
|
||||
}
|
||||
if (path.size() > 0)
|
||||
{
|
||||
SrPnt2 p = path[path.size() - 1];
|
||||
sr_out << "(" << p.x << ", " << p.y << ")";
|
||||
}
|
||||
sr_out << "}\n";
|
||||
Print();
|
||||
}
|
||||
|
||||
//definition of FunnelDeque functions }
|
||||
107
source/dcdt/se/FunnelDeque.h
Normal file
107
source/dcdt/se/FunnelDeque.h
Normal file
@ -0,0 +1,107 @@
|
||||
//FunnelDeque.h
|
||||
|
||||
//DJD: definition of FunnelNode class {
|
||||
|
||||
#ifndef FUNNELDEQUE_H
|
||||
#define FUNNELDEQUE_H
|
||||
|
||||
#include "sr_vec2.h"
|
||||
#include "sr_polygon.h"
|
||||
|
||||
//define a floating point value for pi
|
||||
#define PI 3.1415926535897932384626433832795f
|
||||
|
||||
//a node in the funnel deque
|
||||
class FunnelNode
|
||||
{
|
||||
protected:
|
||||
//left and right pointers (doubly-connected)
|
||||
FunnelNode *m_CLeft;
|
||||
FunnelNode *m_CRight;
|
||||
//the vertex represented by this funnel node
|
||||
SrPnt2 m_CPoint;
|
||||
public:
|
||||
//default constructor
|
||||
FunnelNode();
|
||||
//constructor - initializes the vertex coordinates
|
||||
FunnelNode(float x, float y);
|
||||
//constructor - initializes the vertex
|
||||
FunnelNode(const SrPnt2& point);
|
||||
//mutator for the left pointer
|
||||
void Left(FunnelNode *left);
|
||||
//mutator for the right pointer
|
||||
void Right(FunnelNode *right);
|
||||
//accessor for the left pointer
|
||||
FunnelNode *Left();
|
||||
//accessor for the right pointer
|
||||
FunnelNode *Right();
|
||||
//mutator for the vertex
|
||||
void Point(const SrPnt2& point);
|
||||
//accessor for the vertex
|
||||
SrPnt2 Point();
|
||||
//mutator for the vertex coordinates
|
||||
void Point(float x, float y);
|
||||
//accessor for the vertex x coordinate
|
||||
float X();
|
||||
//accessor for the vertex y coordinate
|
||||
float Y();
|
||||
};
|
||||
|
||||
//definition of FunnelNode class }
|
||||
|
||||
//DJD: definition of FunnelDeque class {
|
||||
|
||||
//the funnel deque used in the funnel algorithm
|
||||
class FunnelDeque
|
||||
{
|
||||
public:
|
||||
//different kinds of corners - point, left and right tangent
|
||||
//(tangent to a circle around the corner, running along the side specified)
|
||||
enum CornerType {Point, LeftTangent, RightTangent};
|
||||
protected:
|
||||
//left end, right end, and apex pointers for the deque
|
||||
FunnelNode *m_CLeft;
|
||||
FunnelNode *m_CRight;
|
||||
FunnelNode *m_CApex;
|
||||
//the corner type of the node at the apex
|
||||
//(ones on the left side are all right tangent, ones on the right are all left tangent)
|
||||
CornerType m_tApexType;
|
||||
//radius of the unit for which we are finding the path
|
||||
float m_dRadius;
|
||||
//angle between the first and second points given their corner types
|
||||
float Angle(float x_1, float y_1, CornerType type_1, float x_2, float y_2, CornerType type_2);
|
||||
//constrains an angfle to the range (-PI, PI]
|
||||
float AngleRange(float theta);
|
||||
//returns the (Euclidean) distance between two points
|
||||
float Distance(float x_1, float y_1, float x_2, float y_2);
|
||||
//returns the angle between the given points
|
||||
//(same as the angle to their tangents if they're the same type)
|
||||
float ToTangent(float x_1, float y_1, float x_2, float y_2);
|
||||
//returns the angle between tangents of the given points
|
||||
//given whether the first is left tangent type, assuming the second is the opposite
|
||||
float ToTangentAlt(float x_1, float y_1, float x_2, float y_2, bool LeftRight);
|
||||
//returns the angle between the first point and a tangent to the second
|
||||
//(the type of the second is specified)
|
||||
float PointToTangent(float x_1, float y_1, float x_2, float y_2, bool Right);
|
||||
//adds a given point to the path
|
||||
void AddPoint(float x, float y, CornerType type, float angle, SrPolygon &path);
|
||||
public:
|
||||
//creates the funnel deque based on a starting point and radius
|
||||
FunnelDeque(float x, float y, float r);
|
||||
//copy constructor
|
||||
FunnelDeque(const SrPnt2& point, float r);
|
||||
//destructor
|
||||
~FunnelDeque();
|
||||
//adds a given point of a given type to the funnel deque, adding to the path if necessary
|
||||
void Add(const SrPnt2& p, CornerType type, SrPolygon& path);
|
||||
//prints the contents of the funnel deque
|
||||
void Print();
|
||||
//prints the contents of the path
|
||||
void Print(SrPolygon path);
|
||||
//calculates the length of the path for a unit of given radius
|
||||
float Length(SrPolygon path);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
//definition of FunnelDeque class }
|
||||
159
source/dcdt/se/Location.cpp
Normal file
159
source/dcdt/se/Location.cpp
Normal file
@ -0,0 +1,159 @@
|
||||
//Location.cpp
|
||||
|
||||
#include "precompiled.h"
|
||||
|
||||
#include <cstdlib>
|
||||
#include <math.h>
|
||||
#include "se_dcdt.h"
|
||||
#include <fstream>
|
||||
|
||||
//the list of unprocessed triangles
|
||||
template <class T>
|
||||
SrArray<SeLinkFace<T> *> SeLinkFace<T>::processing;
|
||||
|
||||
//DJD: Point location function definitions {
|
||||
|
||||
//initializes each sector midpoint to point to the triangle in which it is contained
|
||||
void SeDcdt::InitializeSectors()
|
||||
{
|
||||
#if defined EXPERIMENT
|
||||
//count the number of triangles
|
||||
int num = 0;
|
||||
#endif
|
||||
//calculate the width and height of the triangulation
|
||||
float width = _xmax - _xmin;
|
||||
float height = _ymax - _ymin;
|
||||
//calculate the width and height of each sector
|
||||
sectorWidth = width / (float)xSectors;
|
||||
sectorHeight = height / (float)ySectors;
|
||||
//initialize all sectors to null
|
||||
for (int i = 0; i < ySectors; i++)
|
||||
{
|
||||
for (int j = 0; j < xSectors; j++)
|
||||
{
|
||||
sectors[i][j] = NULL;
|
||||
}
|
||||
}
|
||||
float x, y;
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
//go through the unprocessed triangles
|
||||
for (int k = 0; k < SeDcdtFace::Faces(); k++)
|
||||
{
|
||||
//retrieve the next valid triangle
|
||||
SeDcdtFace *currentFace = SeDcdtFace::Face(k);
|
||||
if (currentFace == NULL)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
#if defined EXPERIMENT
|
||||
//keep track of the number of triangles
|
||||
num++;
|
||||
#endif
|
||||
//get the minimum and maximum x and y values
|
||||
TriangleVertices(currentFace, x1, y1, x2, y2, x3, y3);
|
||||
float left = Min(x1, x2, x3);
|
||||
float right = Max(x1, x2, x3);
|
||||
float top = Min(y1, y2, y3);
|
||||
float bottom = Max(y1, y2, y3);
|
||||
//calculate the minimum and maximum x and y sector indices covered
|
||||
int xIndexMin = (int)((left - _xmin + 0.5f * sectorWidth) / sectorWidth);
|
||||
int xIndexMax = (int)((right - _xmin - 0.5f * sectorWidth) / sectorWidth);
|
||||
int yIndexMin = (int)((top - _ymin + 0.5f * sectorHeight) / sectorHeight);
|
||||
int yIndexMax = (int)((bottom - _ymin - 0.5f * sectorHeight) / sectorHeight);
|
||||
//go through the covered sector midpoints
|
||||
for (int i = yIndexMin; i <= yIndexMax; i++)
|
||||
{
|
||||
for (int j = xIndexMin; j <= xIndexMax; j++)
|
||||
{
|
||||
//calculate the location of the midpoint
|
||||
x = ((float)j + 0.5f) * sectorWidth + _xmin;
|
||||
y = ((float)i + 0.5f) * sectorHeight + _ymin;
|
||||
//if the midpoint is in the current triangle,
|
||||
if (InTriangle(currentFace, x, y))
|
||||
{
|
||||
//point the sector to that triangle
|
||||
sectors[i][j] = currentFace;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#if defined EXPERIMENT
|
||||
//if we are running an experiment, output the number of triangles to a data file
|
||||
std::ofstream outFile("Data.txt", std::ios_base::app);
|
||||
outFile << "Triangles: " << num << "\t";
|
||||
outFile.close();
|
||||
#endif
|
||||
}
|
||||
|
||||
//check if the given point is within a given triangle
|
||||
bool SeDcdt::InTriangle(SeDcdtFace *face, float x, float y)
|
||||
{
|
||||
//get the triangle's vertices
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(face, x1, y1, x2, y2, x3, y3);
|
||||
//check that the point is counterclockwise of all edges
|
||||
return ((Orientation(x1, y1, x2, y2, x, y) >= 0)
|
||||
&& (Orientation(x2, y2, x3, y3, x, y) >= 0)
|
||||
&& (Orientation(x3, y3, x1, y1, x, y) >= 0));
|
||||
}
|
||||
|
||||
//uses the sectors to locate in which triangle the given point lies
|
||||
SeTriangulator::LocateResult SeDcdt::LocatePoint(float x, float y, SeBase* &result)
|
||||
{
|
||||
//calculates the indices of the sector in which the point lies
|
||||
int xIndex = (int)((x - _xmin) / sectorWidth);
|
||||
int yIndex = (int)((y - _ymin) / sectorHeight);
|
||||
//makes sure the indices are within the proper bounds
|
||||
xIndex = (xIndex < 0) ? 0 : (xIndex >= xSectors) ? xSectors - 1 : xIndex;
|
||||
yIndex = (yIndex < 0) ? 0 : (yIndex >= ySectors) ? ySectors - 1 : yIndex;
|
||||
//retrieve the triangle in which that sector midpoint is located
|
||||
SeFace *iniface = sectors[yIndex][xIndex];
|
||||
//if it was invalid, start at the usual place
|
||||
if (iniface == NULL)
|
||||
{
|
||||
iniface = _search_face();
|
||||
}
|
||||
//use this starting triangle to start point location
|
||||
return _triangulator->locate_point(iniface, x, y, result);
|
||||
}
|
||||
|
||||
//locate the given point starting at the usual place
|
||||
SeTriangulator::LocateResult SeDcdt::LocatePointOld(float x, float y, SeBase* &result)
|
||||
{
|
||||
return _triangulator->locate_point(_search_face(), x, y, result);
|
||||
}
|
||||
|
||||
//returns the maximum of the 3 values passed
|
||||
float SeDcdt::Max(float a, float b, float c)
|
||||
{
|
||||
return ((a >= b) && (a >= c)) ? a :
|
||||
((b >= a) && (b >= c)) ? b : c;
|
||||
}
|
||||
|
||||
//returns the minimum of the 3 values passed
|
||||
float SeDcdt::Min(float a, float b, float c)
|
||||
{
|
||||
return ((a <= b) && (a <= c)) ? a :
|
||||
((b <= a) && (b <= c)) ? b : c;
|
||||
}
|
||||
|
||||
//return a random value in the range of x values (used for testing)
|
||||
float SeDcdt::RandomX()
|
||||
{
|
||||
return RandomBetween(_xmin, _xmax);
|
||||
}
|
||||
|
||||
//return a random value in the range of y values (used for testing)
|
||||
float SeDcdt::RandomY()
|
||||
{
|
||||
return RandomBetween(_ymin, _ymax);
|
||||
}
|
||||
|
||||
//returns a random number between the values given
|
||||
float SeDcdt::RandomBetween(float min, float max)
|
||||
{
|
||||
float base = (float)rand() / (float)RAND_MAX;
|
||||
return (base * (max - min) + min);
|
||||
}
|
||||
|
||||
//Point location function definitions }
|
||||
2328
source/dcdt/se/Search.cpp
Normal file
2328
source/dcdt/se/Search.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1545
source/dcdt/se/Search.cpp.bak
Normal file
1545
source/dcdt/se/Search.cpp.bak
Normal file
File diff suppressed because it is too large
Load Diff
135
source/dcdt/se/SearchNode.h
Normal file
135
source/dcdt/se/SearchNode.h
Normal file
@ -0,0 +1,135 @@
|
||||
//SearchNode.h
|
||||
|
||||
//DJD: definition of the SearchNode class {
|
||||
|
||||
#ifndef SEARCHNODE_H
|
||||
#define SEARCHNODE_H
|
||||
|
||||
#include "se_dcdt.h"
|
||||
|
||||
//class used to store a state in the search functions
|
||||
class SearchNode
|
||||
{
|
||||
protected:
|
||||
//the (minimum) distance travelled since the start node
|
||||
float m_dMinDistance;
|
||||
//the (admissible and consistent) estimate of the distance to the goal
|
||||
float m_dHeuristic;
|
||||
//the closest point in this triangle to the goal
|
||||
SrPnt2 m_CClosestPoint;
|
||||
//the triangle (face) associated with this node
|
||||
SeDcdtFace *m_CTriangle;
|
||||
//the parent search node of this one
|
||||
SearchNode *m_CBack;
|
||||
//the number of open children
|
||||
int m_nOpenChildren;
|
||||
//the direction taken from the last node to this one
|
||||
int m_nDirection;
|
||||
public:
|
||||
//constructor - initializes the member variables
|
||||
SearchNode(float dMinDistance, float dHeuristic, SrPnt2 CClosestPoint, SeDcdtFace *CTriangle, SearchNode *CBack, int nDirection = INVALID)
|
||||
{
|
||||
m_dMinDistance = dMinDistance;
|
||||
m_dHeuristic = dHeuristic;
|
||||
m_CClosestPoint.x = CClosestPoint.x;
|
||||
m_CClosestPoint.y = CClosestPoint.y;
|
||||
m_CTriangle = CTriangle;
|
||||
m_CBack = CBack;
|
||||
m_nOpenChildren = 0;
|
||||
m_nDirection = nDirection;
|
||||
}
|
||||
|
||||
//returns the estimated minimum cost of a path from the start to the goal through this node
|
||||
float f()
|
||||
{
|
||||
return (m_dMinDistance + m_dHeuristic);
|
||||
}
|
||||
|
||||
//returns the estimated minimum cost of a path from the start to this node
|
||||
float g()
|
||||
{
|
||||
return m_dMinDistance;
|
||||
}
|
||||
|
||||
//returns the estimated minimum cost of a path from this node to the goal
|
||||
float h()
|
||||
{
|
||||
return m_dHeuristic;
|
||||
}
|
||||
|
||||
//returns (a copy of) the closest point in this triangle to the goal
|
||||
SrPnt2 Point()
|
||||
{
|
||||
SrPnt2 temp;
|
||||
temp.x = m_CClosestPoint.x;
|
||||
temp.y = m_CClosestPoint.y;
|
||||
return temp;
|
||||
}
|
||||
|
||||
//returns the triangle associated with this node
|
||||
SeDcdtFace *Triangle()
|
||||
{
|
||||
return m_CTriangle;
|
||||
}
|
||||
|
||||
//returns the parent search node of this one
|
||||
SearchNode *Back()
|
||||
{
|
||||
return m_CBack;
|
||||
}
|
||||
|
||||
//returns the direction taken from the last node to this one
|
||||
int Direction()
|
||||
{
|
||||
return m_nDirection;
|
||||
}
|
||||
|
||||
//sets the number of open children
|
||||
void OpenChild()
|
||||
{
|
||||
m_nOpenChildren++;
|
||||
}
|
||||
|
||||
//closes a child
|
||||
bool CloseChild()
|
||||
{
|
||||
return ((--m_nOpenChildren) <= 0);
|
||||
}
|
||||
|
||||
//closes the current search node
|
||||
//(deleting its parent if it has no other open children)
|
||||
void Close()
|
||||
{
|
||||
if ((m_CBack != NULL) && m_CBack->CloseChild())
|
||||
{
|
||||
m_CBack->Close();
|
||||
delete m_CBack;
|
||||
m_CBack = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
//returns the number of open children this node has
|
||||
int OpenChildren()
|
||||
{
|
||||
return m_nOpenChildren;
|
||||
}
|
||||
|
||||
//checks the ancestors of this node to see if a given triangle was already searched
|
||||
bool Searched(SeDcdtFace *triangle)
|
||||
{
|
||||
SearchNode *current = m_CBack;
|
||||
while (current != NULL)
|
||||
{
|
||||
if (current->Triangle() == triangle)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
current = current->Back();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
//definition of the SearchNode class }
|
||||
248
source/dcdt/se/Utility.cpp
Normal file
248
source/dcdt/se/Utility.cpp
Normal file
@ -0,0 +1,248 @@
|
||||
//Utility.cpp
|
||||
|
||||
//DJD: Helper function definitions {
|
||||
|
||||
#include "precompiled.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#include "se_dcdt.h"
|
||||
|
||||
//degree value for an unabstracted triangle
|
||||
#define UNABSTRACTED -1
|
||||
|
||||
//the list of unprocessed triangles
|
||||
template <class T>
|
||||
SrArray<SeLinkFace<T> *> SeLinkFace<T>::processing;
|
||||
|
||||
//gets the vertex coordinates and whether or not the edges are blocked, for a face
|
||||
void SeDcdt::TriangleInfo(SeDcdtFace *face, float &x1, float &y1, bool &e1,
|
||||
float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3)
|
||||
{
|
||||
TriangleInfo(face->se(), x1, y1, e1, x2, y2, e2, x3, y3, e3);
|
||||
}
|
||||
|
||||
//gets the vertex coordinates and whether or not the edges are blocked, in a certain order
|
||||
void SeDcdt::TriangleInfo(SeBase *s, float &x1, float &y1, bool &e1,
|
||||
float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3)
|
||||
{
|
||||
TriangleVertices(s, x1, y1, x2, y2, x3, y3);
|
||||
TriangleEdges(s, e1, e2, e3);
|
||||
}
|
||||
|
||||
//gets the vertex coordinates of a triangle, for a face
|
||||
void SeDcdt::TriangleVertices(SeDcdtFace *face, float &x1, float &y1,
|
||||
float &x2, float &y2, float &x3, float &y3)
|
||||
{
|
||||
TriangleVertices(face->se(), x1, y1, x2, y2, x3, y3);
|
||||
}
|
||||
|
||||
//gets the vertex coordinates of a triangle, in a certain order
|
||||
void SeDcdt::TriangleVertices(SeBase *s, float &x1, float &y1,
|
||||
float &x2, float &y2, float &x3, float &y3)
|
||||
{
|
||||
x1 = ((SeDcdtVertex *)s->vtx())->p.x;
|
||||
y1 = ((SeDcdtVertex *)s->vtx())->p.y;
|
||||
s = s->nxt();
|
||||
x2 = ((SeDcdtVertex *)s->vtx())->p.x;
|
||||
y2 = ((SeDcdtVertex *)s->vtx())->p.y;
|
||||
s = s->nxt();
|
||||
x3 = ((SeDcdtVertex *)s->vtx())->p.x;
|
||||
y3 = ((SeDcdtVertex *)s->vtx())->p.y;
|
||||
}
|
||||
|
||||
//gets whether or not the edges of a triangle are blocked, for a face
|
||||
void SeDcdt::TriangleEdges(SeDcdtFace *face, bool &e1, bool &e2, bool &e3)
|
||||
{
|
||||
TriangleEdges(face->se(), e1, e2, e3);
|
||||
}
|
||||
|
||||
//gets whether or not the edges of a triangle are blocked, in a certain order
|
||||
void SeDcdt::TriangleEdges(SeBase *s, bool &e1, bool &e2, bool &e3)
|
||||
{
|
||||
e1 = Blocked(s);
|
||||
s = s->nxt();
|
||||
e2 = Blocked(s);
|
||||
s = s->nxt();
|
||||
e3 = Blocked(s);
|
||||
}
|
||||
|
||||
//gets the midpoint of a triangle
|
||||
void SeDcdt::TriangleMidpoint(SeDcdtFace *face, float &x, float &y)
|
||||
{
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(face, x1, y1, x2, y2, x3, y3);
|
||||
x = (x1 + x2 + x3) / 3.0f;
|
||||
y = (y1 + y2 + y3) / 3.0f;
|
||||
}
|
||||
|
||||
//gets the midpoint of a triangle
|
||||
SrPnt2 SeDcdt::TriangleMidpoint(SeDcdtFace *face)
|
||||
{
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(face, x1, y1, x2, y2, x3, y3);
|
||||
SrPnt2 p;
|
||||
p.x = (x1 + x2 + x3) / 3.0f;
|
||||
p.y = (y1 + y2 + y3) / 3.0f;
|
||||
return p;
|
||||
}
|
||||
|
||||
//gets whether or not an edge is constrained or bordering the outside face
|
||||
bool SeDcdt::Blocked(SeBase *s)
|
||||
{
|
||||
return (((SeDcdtEdge *)s->edg())->is_constrained() || (s->sym()->fac() == outside));
|
||||
}
|
||||
|
||||
//gets the degree of node a face has been abstracted to
|
||||
int SeDcdt::Degree(SeBase *s)
|
||||
{
|
||||
return Degree(s->fac());
|
||||
}
|
||||
|
||||
//gets the degree of node a face has been abstracted to
|
||||
int SeDcdt::Degree(SeFace *face)
|
||||
{
|
||||
if (((SeDcdtFace *)face)->link == NULL)
|
||||
{
|
||||
return UNABSTRACTED;
|
||||
}
|
||||
return ((SeDcdtFace *)face)->link->Degree();
|
||||
}
|
||||
|
||||
//gets the angle between the given vertex coordinates
|
||||
float SeDcdt::AngleBetween(float x1, float y1, float x2, float y2, float x3, float y3)
|
||||
{
|
||||
float angle1 = (float)atan2(y1 - y2, x1 - x2);
|
||||
float angle2 = (float)atan2(y3 - y2, x3 - x2);
|
||||
float diff = angle1 - angle2;
|
||||
diff += (diff <= -PI) ? 2.0f * PI : (diff > PI) ? -2.0f * PI : 0.0f;
|
||||
return diff;
|
||||
}
|
||||
|
||||
//gets the orientation of the vertices with coordinates given
|
||||
//(0 for collinear, <0 for clockwise, >0 for counterclockwise)
|
||||
float SeDcdt::Orientation(float x1, float y1, float x2, float y2, float x3, float y3)
|
||||
{
|
||||
return ((x1 * y2) + (x2 * y3) + (x3 * y1) - (x1 * y3) - (x2 * y1) - (x3 * y2));
|
||||
}
|
||||
|
||||
//determines if an angle is accute
|
||||
bool SeDcdt::IsAccute(float theta)
|
||||
{
|
||||
return (abs(theta) < (PI / 2.0f));
|
||||
}
|
||||
|
||||
//determines if an angle is obtuse
|
||||
bool SeDcdt::IsObtuse(float theta)
|
||||
{
|
||||
return (abs(theta) > (PI / 2.0f));
|
||||
}
|
||||
|
||||
//calculates the distance between two points
|
||||
float SeDcdt::Length(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
float xDiff = x1 - x2;
|
||||
float yDiff = y1 - y2;
|
||||
return (sqrt(xDiff * xDiff + yDiff * yDiff));
|
||||
}
|
||||
|
||||
//returns the smaller of the two arguments
|
||||
float SeDcdt::Minimum(float a, float b)
|
||||
{
|
||||
return ((a < b) ? a : b);
|
||||
}
|
||||
|
||||
//returns the larger of the two arguments
|
||||
float SeDcdt::Maximum(float a, float b)
|
||||
{
|
||||
return ((a > b) ? a : b);
|
||||
}
|
||||
|
||||
//returns the minimum distance between the two segments given
|
||||
float SeDcdt::SegmentDistance(float A1x, float A1y, float A2x, float A2y,
|
||||
float B1x, float B1y, float B2x, float B2y)
|
||||
{
|
||||
float d1 = PointSegmentDistance(A1x, A1y, B1x, B1y, B2x, B2y);
|
||||
float d2 = PointSegmentDistance(A2x, A2y, B1x, B1y, B2x, B2y);
|
||||
float d3 = PointSegmentDistance(B1x, B1y, A1x, A1y, A2x, A2y);
|
||||
float d4 = PointSegmentDistance(B2x, B2y, A1x, A1y, A2x, A2y);
|
||||
float min = Minimum(Minimum(d1, d2), Minimum(d3, d4));
|
||||
return min;
|
||||
}
|
||||
|
||||
//returns the minimum distance between the two segments given, at least r from the endpoints
|
||||
float SeDcdt::SegmentDistance(float ls1x, float ls1y, float ls2x, float ls2y,
|
||||
float ll1x, float ll1y, float ll2x, float ll2y, float r)
|
||||
{
|
||||
float theta = atan2(ls2y - ls1y, ls2x - ls1x);
|
||||
float A1x = ls1x + cos(theta) * r;
|
||||
float A1y = ls1y + sin(theta) * r;
|
||||
float A2x = ls2x - cos(theta) * r;
|
||||
float A2y = ls2y - sin(theta) * r;
|
||||
theta = atan2(ll2y - ll1y, ll2x - ll1x);
|
||||
float B1x = ll1x + cos(theta) * r;
|
||||
float B1y = ll1y + sin(theta) * r;
|
||||
float B2x = ll2x - cos(theta) * r;
|
||||
float B2y = ll2y - sin(theta) * r;
|
||||
return SegmentDistance(A1x, A1y, A2x, A2y, B1x, B1y, B2x, B2y);
|
||||
}
|
||||
|
||||
//returns the minimum distance between the point and the line segment given
|
||||
float SeDcdt::PointSegmentDistance(float x, float y, float x1, float y1, float x2, float y2)
|
||||
{
|
||||
if (!IsAccute(AngleBetween(x1, y1, x2, y2, x, y)))
|
||||
{
|
||||
return Length(x2, y2, x, y);
|
||||
}
|
||||
else if (!IsAccute(AngleBetween(x2, y2, x1, y1, x, y)))
|
||||
{
|
||||
return Length(x1, y1, x, y);
|
||||
}
|
||||
else
|
||||
{
|
||||
return PointLineDistance(x, y, x1, y1, x2, y2);
|
||||
}
|
||||
}
|
||||
|
||||
//returns the closest point on the segment given, at least r in from the endpoints, to the point given
|
||||
SrPnt2 SeDcdt::ClosestPointOn(float x1, float y1, float x2, float y2, float x, float y, float r)
|
||||
{
|
||||
SrPnt2 point;
|
||||
float distance;
|
||||
//checks if the angles along this edge and to the point are accute
|
||||
bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y));
|
||||
bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y));
|
||||
if (accute1 && accute2)
|
||||
{
|
||||
//if both are, get closest point on this line
|
||||
distance = PointLineDistance(x, y, x1, y1, x2, y2);
|
||||
float theta = atan2(y2 - y1, x2 - x1);
|
||||
theta += (Orientation(x, y, x1, y1, x2, y2) > 0.0f) ? PI / -2.0f : PI / 2.0f;
|
||||
theta += (theta <= -PI) ? 2.0f * PI : (theta > PI) ? -2.0f * PI : 0.0f;
|
||||
point.set(x + cos(theta) * distance, y + sin(theta) * distance);
|
||||
//if it's < r from either end, move it out to distance r
|
||||
if (Length(point.x, point.y, x1, y1) < r)
|
||||
{
|
||||
accute2 = false;
|
||||
}
|
||||
else if (Length(point.x, point.y, x2, y2) < r)
|
||||
{
|
||||
accute1 = false;
|
||||
}
|
||||
}
|
||||
if (!accute1 && accute2)
|
||||
{
|
||||
//get point distance r from (x2, y2) in the direction of (x1, y1)
|
||||
float theta = atan2(y1 - y2, x1 - x2);
|
||||
point.set(x2 + cos(theta) * r, y2 + sin(theta) * r);
|
||||
}
|
||||
else if (accute1 && !accute2)
|
||||
{
|
||||
//get point distance r from (x1, y1) in the direction of (x2, y2)
|
||||
float theta = atan2(y2 - y1, x2 - x1);
|
||||
point.set(x1 + cos(theta) * r, y1 + sin(theta) * r);
|
||||
}
|
||||
return point;
|
||||
}
|
||||
|
||||
//Helper function definitions }
|
||||
149
source/dcdt/se/Width.cpp
Normal file
149
source/dcdt/se/Width.cpp
Normal file
@ -0,0 +1,149 @@
|
||||
//Width.cpp
|
||||
|
||||
//DJD: Width function definitions {
|
||||
#include "precompiled.h"
|
||||
|
||||
#include "se_dcdt.h"
|
||||
#include <math.h>
|
||||
|
||||
//list of unprocessed triangles
|
||||
template <class T>
|
||||
SrArray<SeLinkFace<T> *> SeLinkFace<T>::processing;
|
||||
|
||||
//determines the distance between a point and the closest point to it on a line
|
||||
float SeDcdt::PointLineDistance(float x, float y, float x1, float y1, float x2, float y2)
|
||||
{
|
||||
if (x1 == x2)
|
||||
{
|
||||
return abs(x - x1);
|
||||
}
|
||||
float rise = y2 - y1;
|
||||
float run = x2 - x1;
|
||||
float intercept = y1 - (rise / run) * x1;
|
||||
float a = rise;
|
||||
float b = -run;
|
||||
float c = run * intercept;
|
||||
return (abs(a * x + b * y + c) / sqrt(a * a + b * b));
|
||||
}
|
||||
|
||||
//determines if an edge should be considered when calculating the width
|
||||
//(if the base angles of the triangle formed by this edge joined with the "origin" point are both accute)
|
||||
bool SeDcdt::Consider(float x, float y, float x1, float y1, float x2, float y2)
|
||||
{
|
||||
return ((IsAccute(AngleBetween(x, y, x1, y1, x2, y2)))
|
||||
&& (IsAccute(AngleBetween(x, y, x2, y2, x1, y1))));
|
||||
}
|
||||
|
||||
//Calculates the meaningful widths of a triangle
|
||||
void SeDcdt::CalculateWidths(SeDcdtFace *face)
|
||||
{
|
||||
//if the triangle hasn't been abstracted yet, return
|
||||
if (face->link == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
//goes through the vertices of the triangle
|
||||
SeBase *s = face->se();
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//calculate and set the width between those edges
|
||||
face->link->Width(i, TriangleWidth(s->nxt()));
|
||||
s = s->nxt();
|
||||
}
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(face, x1, y1, x2, y2, x3, y3);
|
||||
}
|
||||
|
||||
//determine one width through a triangle
|
||||
float SeDcdt::TriangleWidth(SeBase *s)
|
||||
{
|
||||
//get the coordinates of the triangle
|
||||
float x, y, x1, y1, x2, y2;
|
||||
TriangleVertices(s, x, y, x1, y1, x2, y2);
|
||||
//if either base angle isn't accute, the width of that
|
||||
//triangle is the length of the shorter edge
|
||||
if (!IsAccute(AngleBetween(x, y, x1, y1, x2, y2)))
|
||||
{
|
||||
return Length(x, y, x1, y1);
|
||||
}
|
||||
else if (!IsAccute(AngleBetween(x, y, x2, y2, x1, y1)))
|
||||
{
|
||||
return Length(x, y, x2, y2);
|
||||
}
|
||||
else
|
||||
{
|
||||
//otherwise, calculate the upper bound on the width of the triangle
|
||||
float CurrentWidth = Minimum(Length(x, y, x1, y1), Length(x, y, x2, y2));
|
||||
//and calculate the actual value of the width
|
||||
return SearchWidth(x, y, s->nxt()->sym(), CurrentWidth);
|
||||
}
|
||||
}
|
||||
|
||||
//checks the current triangle for bounds on the triangle width
|
||||
float SeDcdt::SearchWidth(float x, float y, SeBase *s, float CurrentWidth)
|
||||
{
|
||||
//get the coordinates of the triangle
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(s, x1, y1, x2, y2, x3, y3);
|
||||
//checks if the entrance edge should be considered
|
||||
if (!Consider(x, y, x1, y1, x2, y2))
|
||||
{
|
||||
//if not, returns the width unmodified
|
||||
return CurrentWidth;
|
||||
}
|
||||
//calculates the distance between this edge and the "origin" point
|
||||
float Distance = PointLineDistance(x, y, x1, y1, x2, y2);
|
||||
//if it's farther than the upper bound
|
||||
if (Distance >= CurrentWidth)
|
||||
{
|
||||
return CurrentWidth;
|
||||
}
|
||||
//if the edge is constrained,
|
||||
else if (Blocked(s->sym()))
|
||||
{
|
||||
//return this distance as the new upper bound
|
||||
return Distance;
|
||||
}
|
||||
//gets the distance to the vertex opposite the entrance edge
|
||||
Distance = Length(x, y, x3, y3);
|
||||
//if this distance is less than the current upper bound,
|
||||
if (Distance <= CurrentWidth)
|
||||
{
|
||||
//return this distance as the new upper bound
|
||||
return Distance;
|
||||
}
|
||||
//otherwise, searches across the other two edges for bounds
|
||||
CurrentWidth = SearchWidth(x, y, s->nxt()->sym(), CurrentWidth);
|
||||
return SearchWidth(x, y, s->nxt()->nxt()->sym(), CurrentWidth);
|
||||
}
|
||||
|
||||
//determines if a certain point (x, y) in face is at least distance r from any constraints
|
||||
bool SeDcdt::ValidPosition(float x, float y, SeDcdtFace *face, float r)
|
||||
{
|
||||
//get the triangle's vertices
|
||||
float x1, y1, x2, y2, x3, y3;
|
||||
TriangleVertices(face, x1, y1, x2, y2, x3, y3);
|
||||
//check if any are closer than r to (x, y)
|
||||
if ((Length(x, y, x1, y1) < r) || (Length(x, y, x2, y2) < r) || (Length(x, y, x3, y3) < r))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
//get the edges of the triangle
|
||||
SeBase *s = face->se();
|
||||
//go through them one-by-one
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
//if there are any constraints within r of (x, y) across the current edge
|
||||
if (SearchWidth(x, y, s->sym(), r) < r)
|
||||
{
|
||||
//return that this in an invalid position
|
||||
return false;
|
||||
}
|
||||
//move to the next edge
|
||||
s = s->nxt();
|
||||
}
|
||||
//if no constraints were found within r of (x, y), it is a valid position
|
||||
return true;
|
||||
}
|
||||
|
||||
//Width function definitions }
|
||||
163
source/dcdt/se/data/ecol.se
Normal file
163
source/dcdt/se/data/ecol.se
Normal file
@ -0,0 +1,163 @@
|
||||
SYMEDGE MESH DESCRIPTION
|
||||
|
||||
SymEdges 76
|
||||
37 54 0 0 0
|
||||
24 3 14 0 8
|
||||
1 5 1 1 8
|
||||
35 65 14 1 18
|
||||
2 7 2 2 8
|
||||
67 62 1 2 11
|
||||
4 9 3 3 8
|
||||
39 67 2 3 19
|
||||
6 11 4 4 8
|
||||
69 39 3 4 13
|
||||
8 13 5 5 8
|
||||
75 58 4 5 12
|
||||
10 15 6 6 8
|
||||
57 32 5 6 14
|
||||
12 17 7 7 8
|
||||
49 47 6 7 4
|
||||
14 19 8 8 8
|
||||
26 49 7 8 5
|
||||
16 21 9 9 8
|
||||
42 26 8 9 1
|
||||
18 23 10 10 8
|
||||
23 44 9 10 3
|
||||
20 25 11 11 8
|
||||
45 20 10 11 3
|
||||
22 0 0 12 8
|
||||
51 40 11 12 9
|
||||
48 16 8 13 5
|
||||
19 43 12 13 1
|
||||
31 51 0 14 10
|
||||
54 61 13 14 17
|
||||
52 46 12 15 6
|
||||
50 29 13 15 10
|
||||
71 75 5 16 16
|
||||
13 56 15 16 14
|
||||
62 72 17 17 7
|
||||
64 2 1 17 18
|
||||
65 71 15 18 20
|
||||
55 1 14 18 0
|
||||
9 68 16 19 13
|
||||
66 6 3 19 19
|
||||
43 45 11 20 2
|
||||
25 50 12 20 9
|
||||
27 18 9 21 1
|
||||
44 41 12 21 2
|
||||
40 42 9 22 2
|
||||
21 22 11 22 3
|
||||
15 48 12 23 4
|
||||
30 53 6 23 6
|
||||
17 27 12 24 5
|
||||
46 14 7 24 4
|
||||
28 30 12 25 10
|
||||
41 24 0 25 9
|
||||
47 31 13 26 6
|
||||
61 57 6 26 15
|
||||
60 28 0 27 17
|
||||
0 36 15 27 0
|
||||
53 60 15 28 15
|
||||
33 12 6 28 14
|
||||
72 69 4 29 21
|
||||
11 74 17 29 12
|
||||
29 55 15 30 17
|
||||
56 52 13 30 15
|
||||
73 35 1 31 7
|
||||
5 66 16 31 11
|
||||
3 34 17 32 18
|
||||
70 37 14 32 20
|
||||
7 38 16 33 19
|
||||
63 4 2 33 11
|
||||
58 73 16 34 21
|
||||
38 8 4 34 13
|
||||
36 64 17 35 20
|
||||
74 33 15 35 16
|
||||
68 59 17 36 21
|
||||
34 63 16 36 7
|
||||
32 70 17 37 16
|
||||
59 10 5 37 12
|
||||
|
||||
Vertices 18
|
||||
51 -0.471118 -0.471118 0.000000 127 127 127 255
|
||||
62 1.006899 -0.406455 0.000000 127 127 127 255
|
||||
67 1.256315 0.027713 0.000000 127 127 127 255
|
||||
9 1.265552 0.489593 0.000000 127 127 127 255
|
||||
69 0.914523 0.748246 0.000000 127 127 127 255
|
||||
75 0.572732 0.859097 0.000000 127 127 127 255
|
||||
47 -0.147802 0.840622 0.000000 127 127 127 255
|
||||
17 -0.840622 0.840622 0.000000 127 127 127 255
|
||||
19 -1.182414 0.655870 0.000000 127 127 127 255
|
||||
18 -1.487255 0.129327 0.000000 127 127 127 255
|
||||
23 -1.441067 -0.286366 0.000000 127 127 127 255
|
||||
40 -0.932998 -0.434168 0.000000 127 127 127 255
|
||||
50 -0.907948 0.177278 0.000000 127 127 127 255
|
||||
61 -0.390490 0.162904 0.000000 127 127 127 255
|
||||
3 0.498831 -0.498831 0.000000 127 127 127 255
|
||||
36 0.122178 0.138947 0.000000 127 127 127 255
|
||||
73 1.135253 0.143426 -0.000000 127 127 127 255
|
||||
70 0.702544 0.153150 0.000000 127 127 127 255
|
||||
|
||||
Edges 38
|
||||
0 127 127 127 255
|
||||
2 127 127 127 255
|
||||
4 127 127 127 255
|
||||
6 127 127 127 255
|
||||
8 127 127 127 255
|
||||
10 127 127 127 255
|
||||
12 127 127 127 255
|
||||
14 127 127 127 255
|
||||
16 127 127 127 255
|
||||
18 127 127 127 255
|
||||
20 127 127 127 255
|
||||
22 127 127 127 255
|
||||
24 127 127 127 255
|
||||
26 127 127 127 255
|
||||
28 255 14 238 255
|
||||
30 127 127 127 255
|
||||
32 127 127 127 255
|
||||
34 127 127 127 255
|
||||
36 127 127 127 255
|
||||
38 127 127 127 255
|
||||
40 127 127 127 255
|
||||
42 127 127 127 255
|
||||
44 127 127 127 255
|
||||
46 255 72 8 255
|
||||
48 127 127 127 255
|
||||
50 78 100 255 255
|
||||
52 120 255 98 255
|
||||
54 127 127 127 255
|
||||
56 127 127 127 255
|
||||
58 127 127 127 255
|
||||
60 255 17 78 255
|
||||
62 127 127 127 255
|
||||
64 127 127 127 255
|
||||
66 127 127 127 255
|
||||
68 127 127 127 255
|
||||
70 255 22 71 255
|
||||
72 127 127 127 255
|
||||
74 55 8 255 255
|
||||
|
||||
Faces 22
|
||||
37 127 127 127 255
|
||||
19 127 127 127 255
|
||||
40 127 127 127 255
|
||||
23 127 127 127 255
|
||||
49 127 127 127 255
|
||||
17 127 127 127 255
|
||||
47 127 127 127 255
|
||||
73 127 127 127 255
|
||||
24 127 127 127 255
|
||||
51 127 127 127 255
|
||||
50 127 127 127 255
|
||||
67 127 127 127 255
|
||||
75 127 127 127 255
|
||||
69 127 127 127 255
|
||||
13 127 127 127 255
|
||||
61 127 127 127 255
|
||||
74 127 127 127 255
|
||||
29 127 127 127 255
|
||||
3 127 127 127 255
|
||||
7 127 127 127 255
|
||||
36 127 127 127 255
|
||||
68 127 127 127 255
|
||||
24
source/dcdt/se/linux/makefile
Normal file
24
source/dcdt/se/linux/makefile
Normal file
@ -0,0 +1,24 @@
|
||||
|
||||
export ROOT = ../..
|
||||
export INCLUDEDIR = -I $(ROOT)/ -I $(ROOT)/../sr/ -I $(ROOT)/../fltk/fltk-1.1.5rc34
|
||||
export LIBDIR = -L $(ROOT)/lib -L $(ROOT)/../sr/lib -L $(ROOT)/../fltk/fltk-1.1.5rc3/lib
|
||||
export LIBS = -lse -lsrfl -lsrgl -lsr -lfltk -lfltk_gl -lGL
|
||||
|
||||
DIRS = se semesh setut
|
||||
|
||||
export CC = g++
|
||||
export CFLAGS = $(INCLUDEDIR)
|
||||
export LFLAGS = $(LIBDIR) $(LIBS)
|
||||
|
||||
all:
|
||||
@for dir in $(DIRS); do\
|
||||
mkdir $$dir; cd $$dir; $(MAKE) -f ../makefile.$$dir; cd ..; \
|
||||
done
|
||||
|
||||
clean:
|
||||
-$(RM) core *.o *~ ../lib/* ../bin/*
|
||||
@for dir in $(DIRS); do \
|
||||
rm -r $$dir; \
|
||||
done
|
||||
|
||||
|
||||
14
source/dcdt/se/linux/makefile.se
Normal file
14
source/dcdt/se/linux/makefile.se
Normal file
@ -0,0 +1,14 @@
|
||||
|
||||
SRCDIR = $(ROOT)/src/se/
|
||||
LIB = $(ROOT)/lib/libse.a
|
||||
|
||||
CPPFILES := $(shell echo $(SRCDIR)*.cpp)
|
||||
OBJFILES = $(CPPFILES:.cpp=.o)
|
||||
OBJECTS = $(notdir $(OBJFILES))
|
||||
|
||||
$(LIB): $(OBJECTS)
|
||||
ar -r $(LIB) $(OBJECTS)
|
||||
|
||||
%.o: $(SRCDIR)%.cpp
|
||||
$(CC) -c $(CFLAGS) $< -o $@
|
||||
|
||||
14
source/dcdt/se/linux/makefile.semesh
Normal file
14
source/dcdt/se/linux/makefile.semesh
Normal file
@ -0,0 +1,14 @@
|
||||
|
||||
SRCDIR = $(ROOT)/src/semesh/
|
||||
BIN = $(ROOT)/bin/semesh
|
||||
|
||||
CPPFILES := $(shell echo $(SRCDIR)*.cpp)
|
||||
OBJFILES = $(CPPFILES:.cpp=.o)
|
||||
OBJECTS = $(notdir $(OBJFILES))
|
||||
|
||||
$(BIN): $(OBJECTS)
|
||||
$(CC) $(OBJECTS) $(LFLAGS) -o $(BIN)
|
||||
|
||||
%.o: $(SRCDIR)%.cpp
|
||||
$(CC) -c $(CFLAGS) $< -o $@
|
||||
|
||||
14
source/dcdt/se/linux/makefile.setut
Normal file
14
source/dcdt/se/linux/makefile.setut
Normal file
@ -0,0 +1,14 @@
|
||||
|
||||
SRCDIR = $(ROOT)/src/setut/
|
||||
BIN = $(ROOT)/bin/setut
|
||||
|
||||
CPPFILES := $(shell echo $(SRCDIR)*.cpp)
|
||||
OBJFILES = $(CPPFILES:.cpp=.o)
|
||||
OBJECTS = $(notdir $(OBJFILES))
|
||||
|
||||
$(BIN): $(OBJECTS)
|
||||
$(CC) $(OBJECTS) $(LFLAGS) -o $(BIN)
|
||||
|
||||
%.o: $(SRCDIR)%.cpp
|
||||
$(CC) -c $(CFLAGS) $< -o $@
|
||||
|
||||
70
source/dcdt/se/readme.txt
Normal file
70
source/dcdt/se/readme.txt
Normal file
@ -0,0 +1,70 @@
|
||||
|
||||
===============================================================
|
||||
SR - Simulation and Representation Toolkit
|
||||
(also known as the small scene graph toolkit)
|
||||
|
||||
Marcelo Kallmann 1995-2004
|
||||
|
||||
See file sr.h for compilation options and
|
||||
important notes about programming conventions
|
||||
===============================================================
|
||||
|
||||
==== LIBRARIES ====
|
||||
|
||||
- sr: stand alone SR library with all main classes
|
||||
- srgl: OpenGL rendering of the SR scene nodes (www.opengl.org)
|
||||
- srfl: FLTK-OpenGL viewers and other UI tools (www.fltk.org)
|
||||
|
||||
==== EXECUTABLES ====
|
||||
|
||||
- srmodel: demonstration and processing of SrModel
|
||||
- srtest: several small programs to test/demonstrate
|
||||
the toolkit. All tests are compiled in the
|
||||
same executable, but only one test is called
|
||||
from srtest_main.cpp.
|
||||
- srxapp: Example application to start a project containing
|
||||
a FLTK user interface and the 3D viewer SrViewer,
|
||||
which uses FLTK and OpenGL.
|
||||
|
||||
==== VISUALC ====
|
||||
|
||||
- Configurations Debug and Release are the usual ones, and
|
||||
configuration Compact is meant to build executables
|
||||
without any dependencies on DLLs.
|
||||
- Generated libs are: sr, srfl, srgl (release)
|
||||
src, srflc, srglc (compact), and
|
||||
srd, srfld, srgld (debug)
|
||||
- Folder visualc6 contains projects for Visual C++ 6
|
||||
- Folder visualc7 contains projects for Visual C++ .NET
|
||||
- FLTK include and libs are set to: ..\..\fltk
|
||||
|
||||
==== LINUX ====
|
||||
|
||||
- the linux folder contains makefiles for compilation using
|
||||
gnu g++ and gmake tools.
|
||||
- Some makefiles will look for FLTK as specified in the
|
||||
main makefile linux/makefile
|
||||
- gmake must be called from inside the linux/ directory
|
||||
|
||||
==== TESTED PLATFORMS ====
|
||||
|
||||
- Windows 98 with Visual C++ 6.0
|
||||
- Windows XP with Visual C++ .NET
|
||||
- Linux with gmake and g++
|
||||
|
||||
==== KNOWN BUGS ====
|
||||
|
||||
- check if SrSnEditor::~SrSnEditor() bug is really fixed
|
||||
|
||||
- OpenGL: Transparency is currently disabled in SrViewer,
|
||||
because it had side-effects with "solid" polygons.
|
||||
|
||||
- SrViewer: Need to review/fix camera control and view_all()
|
||||
|
||||
==== WISH LIST ====
|
||||
|
||||
- Compare performance of using double or floats in sr_bv_math.h
|
||||
- Finish texture support in SrModel
|
||||
- SrPolygon: make a better grow() method
|
||||
- Make a good trackball manipulator scene node
|
||||
|
||||
31
source/dcdt/se/se.cpp
Normal file
31
source/dcdt/se/se.cpp
Normal file
@ -0,0 +1,31 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include "se.h"
|
||||
|
||||
//================================= SeElement ====================================
|
||||
|
||||
SeElement::SeElement ()
|
||||
{
|
||||
_index=0;
|
||||
_symedge=0;
|
||||
_next=_prior=this;
|
||||
}
|
||||
|
||||
SeElement* SeElement::_remove ()
|
||||
{
|
||||
_next->_prior = _prior;
|
||||
_prior->_next = _next;
|
||||
return this;
|
||||
}
|
||||
|
||||
// same insert_prior() implementation as in SrListNode
|
||||
SeElement* SeElement::_insert ( SeElement* n )
|
||||
{
|
||||
n->_prior = _prior;
|
||||
_prior->_next = n;
|
||||
n->_next = this;
|
||||
_prior = n;
|
||||
return n;
|
||||
}
|
||||
|
||||
//=== End of File ===================================================================
|
||||
199
source/dcdt/se/se.h
Normal file
199
source/dcdt/se/se.h
Normal file
@ -0,0 +1,199 @@
|
||||
|
||||
# ifndef SE_H
|
||||
# define SE_H
|
||||
|
||||
/** \file se.h
|
||||
* Sym Edge definitions
|
||||
\code
|
||||
******************************************************************************************
|
||||
*
|
||||
* SymEdge Mesh Data Structure
|
||||
* Marcelo Kallmann 1996 - 2004
|
||||
*
|
||||
* SymEdge joins Guibas & Stolfi structure ideas with Mäntylä Euler operators.
|
||||
* Several triangulation algorithms were implemented in the se toolkit
|
||||
*
|
||||
* References:
|
||||
* 1. M. Kallmann, H. Bieri, and D. Thalmann, "Fully Dynamic Constrained Delaunay
|
||||
* Triangulations", In Geometric Modelling for Scientific Visualization,
|
||||
* G. Brunnett, B. Hamann, H. Mueller, L. Linsen (Eds.), ISBN 3-540-40116-4,
|
||||
* Springer-Verlag, Heidelberg, Germany, pp. 241-257, 2003.
|
||||
* 2. M. Kallmann, and D. Thalmann, "Star-Vertices: A Compact Representation
|
||||
* for Planar Meshes with Adjacency Information", Journal of Graphics Tools,
|
||||
* 2001. (In the comparisons table, the symedge was used and called as the
|
||||
* "simplified quad-edge structure" )
|
||||
* 3. M. Mäntylä, "An Introduction to Solid Modeling", Computer Science Press,
|
||||
* Maryland, 1988.
|
||||
* 4. L. Guibas and J. Stolfi, "Primitives for the Manipulation of General
|
||||
* Subdivisions and the Computation of Voronoi Diagrams", ACM Transaction
|
||||
* on Graphics, 4, 75-123, 1985.
|
||||
*
|
||||
* The structure is based on circular lists of SeElement for vertex/edge/face nodes,
|
||||
* plus the connectivity information stored with SeBase class.
|
||||
*
|
||||
* Implementation History:
|
||||
*
|
||||
* 10/2004 - Becomes again SE lib, but with dependendy on the SR lib.
|
||||
*
|
||||
* 10/2003 - Integration to the SR library, which is the newer verion of FG.
|
||||
*
|
||||
* 08/2002 - SeGeo becomes abstract, and SeGeoPoint, SeGeoFloat, SeGeoPointFloat appear.
|
||||
* - SeTriangulator now covers both constrained and conforming triangulations
|
||||
* - SrClassManagerBase signature is no longer used
|
||||
*
|
||||
* 06/2001 - The library becomes again a standalone library.
|
||||
*
|
||||
* 09/2000 - Changed to always keep v/e/f information lists
|
||||
* - New names starting with Se
|
||||
*
|
||||
* 05/2000 - Finally operator mg was done to convert triangle soups with genus>0
|
||||
*
|
||||
* 10/1999 - Adaptation to the FG library, using FgArray, FgListNode, FgTree, I/O, etc.
|
||||
*
|
||||
* 06/1998 - Abandoned the idea of using a base algorithm class MeshAlg
|
||||
*
|
||||
* 07/1997 - Automatic calculation of MaxindexValue
|
||||
* - Import() and related virtual callbacks created
|
||||
* - Triangulation now is done with virtual callbacks
|
||||
*
|
||||
* 07/1996 - First implementation
|
||||
*
|
||||
******************************************************************************************
|
||||
\endcode */
|
||||
|
||||
//================================ Types and Constants ===============================
|
||||
|
||||
class SeMeshBase;
|
||||
class SeBase;
|
||||
class SeElement;
|
||||
|
||||
typedef unsigned int semeshindex; //!< internally used to mark elements (must be unsigned)
|
||||
typedef SeElement SeVertex; //!< a vertex is an element
|
||||
typedef SeElement SeEdge; //!< an edge is an element
|
||||
typedef SeElement SeFace; //!< a face is an element
|
||||
|
||||
//=================================== SeElement =======================================
|
||||
|
||||
/*! SeElement contains the required information to be attached
|
||||
to all vertices, edges and faces:
|
||||
1. a reference to one (any) adjacent SeBase
|
||||
2. pointers maintaining a circular list of all elements of
|
||||
the same type (vertices, edges or faces)
|
||||
3. an index that can be used by SeMeshBase to mark elements
|
||||
To attach user-related information to an element:
|
||||
1. SeElement must be derived and all user data is declared
|
||||
inside the derived class.
|
||||
2. A corresponding SrClassManager must be derived and
|
||||
the required virtual methods must be re-written in order to
|
||||
manage the derived SeElement class. The compare method
|
||||
is not used. See also sr_class_manager.h. */
|
||||
class SeElement
|
||||
{ protected :
|
||||
SeElement ();
|
||||
public :
|
||||
SeBase* se () const { return _symedge; }
|
||||
SeElement* nxt() const { return _next; }
|
||||
SeElement* pri() const { return _prior; }
|
||||
private :
|
||||
friend class SeMeshBase;
|
||||
friend class SrClassManagerBase;
|
||||
SeElement* _next;
|
||||
SeElement* _prior;
|
||||
SeBase* _symedge;
|
||||
semeshindex _index;
|
||||
SeElement* _remove ();
|
||||
SeElement* _insert ( SeElement* n );
|
||||
};
|
||||
|
||||
/*! The following define can be called in a user derived class of SeElement
|
||||
to easily redefine public SeElement methods with correct type casts.
|
||||
E stands for the element type, and S for the sym edge type. */
|
||||
# define SE_ELEMENT_CASTED_METHODS(E,S) \
|
||||
S* se() const { return (S*)SeElement::se(); } \
|
||||
E* nxt() const { return (E*)SeElement::nxt(); } \
|
||||
E* pri() const { return (E*)SeElement::pri(); }
|
||||
|
||||
/*! The following define can be used to fully declare a default user derived
|
||||
class of SeElement, which contains no user data but correctly redefines
|
||||
public SeElement methods with type casts. */
|
||||
# define SE_DEFINE_DEFAULT_ELEMENT(E,S) \
|
||||
class E : public SeElement \
|
||||
{ public : \
|
||||
SE_ELEMENT_CASTED_METHODS(E,S); \
|
||||
E () {} \
|
||||
E ( const E& e ) {} \
|
||||
friend SrOutput& operator<< ( SrOutput& o, const E& e ) { return o; } \
|
||||
friend SrInput& operator>> ( SrInput& i, E& e ) { return i; } \
|
||||
friend int sr_compare ( const E* e1, const E* e2 ) { return 0; } \
|
||||
};
|
||||
|
||||
//================================== SeBase ========================================
|
||||
|
||||
/*! Used to describe all adjacency relations of the mesh topology. The mesh itself is
|
||||
composed of a net of SeBase elements linked together reflecting the vertex and
|
||||
face loops. SymEdge is a short name for symetrical edge, as each SeBase has a
|
||||
symetrical one incident to the same edge on the opposite face, and is given by
|
||||
sym(). SeBase has local traverse operators permitting to change to any adjacent
|
||||
symedge so to access any information stored on a vertex, edge or face. Symedges
|
||||
are also used as parameters to the topological operators of SeMeshBase, which allow
|
||||
modifying the mesh. */
|
||||
class SeBase
|
||||
{ public :
|
||||
/*! Returns the next symedge adjacent to the same face. */
|
||||
SeBase* nxt() const { return _next; }
|
||||
|
||||
/*! Returns the prior symedge adjacent to the same face. */
|
||||
SeBase* pri() const { return _rotate->_next->_rotate; }
|
||||
|
||||
/*! Returns the next symedge adjacent to the same vertex. */
|
||||
SeBase* rot() const { return _rotate; }
|
||||
|
||||
/*! Returns the prior symedge adjacent to the same vertex. */
|
||||
SeBase* ret() const { return _next->_rotate->_next; }
|
||||
|
||||
/*! Returns the symmetrical symedge, sharing the same edge. */
|
||||
SeBase* sym() const { return _next->_rotate; }
|
||||
|
||||
/*! Returns the element attached to the incident vertex. */
|
||||
SeVertex* vtx() const { return _vertex; }
|
||||
|
||||
/*! Returns the element attached to the incident edge. */
|
||||
SeEdge* edg() const { return _edge; }
|
||||
|
||||
/*! Returns the element attached to the incident face. */
|
||||
SeFace* fac() const { return _face; }
|
||||
|
||||
private :
|
||||
friend class SeMeshBase;
|
||||
friend class SeElement;
|
||||
SeBase* _next;
|
||||
SeBase* _rotate;
|
||||
SeVertex* _vertex;
|
||||
SeEdge* _edge;
|
||||
SeFace* _face;
|
||||
};
|
||||
|
||||
/*! This is the template version of the SeBase class, that redefines the methods
|
||||
of SeBase including all needed type casts to the user defined classes.
|
||||
All methods are implemented inline just calling the corresponding method of
|
||||
the base class but correctly applying type casts to convert default types
|
||||
to user types.
|
||||
Important Note: no user data can be stored in sym edges. This template class
|
||||
is only used as a technique to correctly perform type casts. */
|
||||
template <class V, class E, class F>
|
||||
class Se : public SeBase
|
||||
{ public :
|
||||
Se* nxt() const { return (Se*)SeBase::nxt(); }
|
||||
Se* pri() const { return (Se*)SeBase::pri(); }
|
||||
Se* rot() const { return (Se*)SeBase::rot(); }
|
||||
Se* ret() const { return (Se*)SeBase::ret(); }
|
||||
Se* sym() const { return (Se*)SeBase::sym(); }
|
||||
V* vtx() const { return (V*)SeBase::vtx(); }
|
||||
E* edg() const { return (E*)SeBase::edg(); }
|
||||
F* fac() const { return (F*)SeBase::fac(); }
|
||||
};
|
||||
|
||||
//============================ End of File ===============================
|
||||
|
||||
# endif // SE_H
|
||||
|
||||
709
source/dcdt/se/se_dcdt.cpp
Normal file
709
source/dcdt/se/se_dcdt.cpp
Normal file
@ -0,0 +1,709 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include <math.h>
|
||||
# include <stdlib.h>
|
||||
# include <string.h>
|
||||
# include "sr_box.h"
|
||||
# include "sr_geo2.h"
|
||||
# include "se_dcdt.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // insert polygon
|
||||
//# define SR_USE_TRACE2 // init
|
||||
//# define SR_USE_TRACE3 // insert polygon
|
||||
//# define SR_USE_TRACE4 // remove polygon
|
||||
//# define SR_USE_TRACE5 // funnel
|
||||
//# define SR_USE_TRACE6 // translate
|
||||
//# define SR_USE_TRACE7 // ray
|
||||
//# define SR_USE_TRACE8 // tri manager
|
||||
//# define SR_USE_TRACE9 // extract contours
|
||||
# include "sr_trace.h"
|
||||
|
||||
//DJD: declaring static member variables {
|
||||
|
||||
template <class T>
|
||||
SrArray<SeLinkFace<T> *> SeLinkFace<T>::processing;
|
||||
template <class T>
|
||||
int SeLinkFace<T>::current = 0;
|
||||
|
||||
//declaring static member variables }
|
||||
|
||||
//=============================== SeDcdt ==================================
|
||||
|
||||
SeDcdt::SeDcdt ()
|
||||
{
|
||||
_mesh = new SeDcdtMesh;
|
||||
|
||||
_triangulator = new SeTriangulator
|
||||
( SeTriangulator::ModeConstrained,
|
||||
_mesh,
|
||||
new SeDcdtTriManager,
|
||||
0.000001 /*epsilon*/ );
|
||||
|
||||
_first_symedge = 0;
|
||||
_cur_search_face = 0;
|
||||
_xmin = _xmax = _ymin = _ymax = 0;
|
||||
_radius = -1;
|
||||
}
|
||||
|
||||
SeDcdt::~SeDcdt ()
|
||||
{
|
||||
delete _triangulator;
|
||||
delete _mesh;
|
||||
}
|
||||
|
||||
void SeDcdt::get_mesh_edges ( SrArray<SrPnt2>* constr, SrArray<SrPnt2>* unconstr )
|
||||
{
|
||||
SeDcdtEdge *e, *ei;
|
||||
SeDcdtSymEdge *s;
|
||||
SrArray<SrPnt2>* pa;
|
||||
|
||||
if ( constr ) constr->size(0);
|
||||
if ( unconstr ) unconstr->size(0);
|
||||
|
||||
e = ei = _mesh->first()->edg();
|
||||
|
||||
do { if ( e->is_constrained() ) pa=constr; else pa=unconstr;
|
||||
if ( pa )
|
||||
{ s = e->se(); pa->push() = s->vtx()->p;
|
||||
s = s->nxt(); pa->push() = s->vtx()->p;
|
||||
}
|
||||
e = e->nxt();
|
||||
} while ( e!=ei );
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//================================ save ==========================================
|
||||
//================================================================================
|
||||
|
||||
bool SeDcdt::save ( SrOutput& out )
|
||||
{
|
||||
int elems = _polygons.elements();
|
||||
|
||||
out << "SeDcdt\n\n";
|
||||
out << "# domain:" << (elems>0?1:0) << " polygons:" << (elems-1) << srnl << srnl;
|
||||
out << "epsilon " << _triangulator->epsilon() << srnl << srnl;
|
||||
|
||||
if ( _radius!=-1 )
|
||||
out << "init_radius " << _radius << srnl << srnl;
|
||||
|
||||
if ( elems==0 ) return true;
|
||||
|
||||
int i, id, psize;
|
||||
int maxid = _polygons.maxid();
|
||||
for ( id=0; id<=maxid; id++ )
|
||||
{ if ( !_polygons[id] ) continue;
|
||||
|
||||
SeDcdtInsPol& p = *_polygons[id];
|
||||
|
||||
if ( id==0 )
|
||||
{ out << "domain\n"; }
|
||||
else
|
||||
{ out << "polygon " << id;
|
||||
if ( p.open ) out << " open";
|
||||
out << srnl;
|
||||
}
|
||||
|
||||
psize = p.size();
|
||||
for ( i=0; i<psize; i++ )
|
||||
{ out << p[i]->p;
|
||||
if ( i==psize-1 ) out<<';'; else out<<srspc;
|
||||
}
|
||||
out << srnl << srnl;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//================================ load ==========================================
|
||||
//================================================================================
|
||||
|
||||
static void _read_pol ( SrInput& inp, SrPolygon& pol )
|
||||
{
|
||||
pol.size ( 0 );
|
||||
while ( 1 )
|
||||
{ inp.get_token();
|
||||
if ( inp.last_token()==';' ) break;
|
||||
inp.unget_token ();
|
||||
inp >> pol.push();
|
||||
}
|
||||
}
|
||||
|
||||
bool SeDcdt::load ( SrInput& inp )
|
||||
{
|
||||
SrPolygon pol;
|
||||
pol.capacity ( 64 );
|
||||
|
||||
SrArray<int> invids;
|
||||
|
||||
float epsilon = 0.00001f;
|
||||
float radius = -1.0f;
|
||||
int id, nextid=1;
|
||||
|
||||
// signature:
|
||||
inp.comment_style ( '#' );
|
||||
inp.get_token ();
|
||||
if ( inp.last_token()!="SeDcdt") return false;
|
||||
|
||||
while ( 1 )
|
||||
{ inp.get_token ();
|
||||
if ( inp.finished() ) break;
|
||||
SrString& s = inp.last_token();
|
||||
|
||||
if ( s=="epsilon" )
|
||||
{ inp >> epsilon;
|
||||
}
|
||||
if ( s=="init_radius" )
|
||||
{ inp >> radius;
|
||||
}
|
||||
else if ( s=="domain" )
|
||||
{ _read_pol ( inp, pol );
|
||||
pol.open ( false );
|
||||
init ( pol, epsilon, radius );
|
||||
}
|
||||
else if ( s=="polygon" )
|
||||
{ if ( num_polygons()==0 ) return false;
|
||||
inp >> id;
|
||||
while ( id>nextid ) { invids.push()=_polygons.insert(); nextid++; }
|
||||
inp.get_token();
|
||||
if ( inp.last_token()=="open" )
|
||||
{ pol.open(true); }
|
||||
else
|
||||
{ inp.unget_token();
|
||||
pol.open(false);
|
||||
}
|
||||
_read_pol ( inp, pol );
|
||||
insert_polygon ( pol );
|
||||
nextid++;
|
||||
}
|
||||
}
|
||||
|
||||
while ( invids.size() ) _polygons.remove(invids.pop());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//================================ init ==========================================
|
||||
//================================================================================
|
||||
|
||||
void SeDcdt::init ( const SrPolygon& domain , float epsilon, float radius )
|
||||
{
|
||||
SR_TRACE2 ( "Starting init()..." );
|
||||
|
||||
// Reset cur searched face:
|
||||
_cur_search_face = 0;
|
||||
|
||||
// Clear structures if needed:
|
||||
if ( _first_symedge )
|
||||
{ _mesh->destroy(); _first_symedge=0; _polygons.init(); }
|
||||
|
||||
// Checks parameters:
|
||||
if ( domain.size()<3 || domain.open() )
|
||||
sr_out.fatal_error ( "SeDcdt::init(): domain polygon must be simple and closed!");
|
||||
|
||||
// Calculates an external polygon (the border) :
|
||||
SR_TRACE2 ( "Calculating External Polygon for domain with "<<domain.size()<<" points..." );
|
||||
SrBox b;
|
||||
domain.get_bounding_box ( b );
|
||||
_xmin=b.a.x; _xmax=b.b.x; _ymin=b.a.y; _ymax=b.b.y;
|
||||
float dx = (_xmax-_xmin)/5.0f;
|
||||
float dy = (_ymax-_ymin)/5.0f;
|
||||
if ( radius>0 )
|
||||
{ if ( dx<radius ) dx=radius;
|
||||
if ( dy<radius ) dy=radius;
|
||||
}
|
||||
if ( radius!=0 )
|
||||
{ _xmax+=dx; _xmin-=dx; _ymax+=dy; _ymin-=dy; }
|
||||
|
||||
// Creates the triangulated external square (the border):
|
||||
SR_TRACE2 ( "Creating External Polygon..." );
|
||||
_triangulator->epsilon ( epsilon );
|
||||
_first_symedge = (SeDcdtSymEdge*)_triangulator->init_as_triangulated_square
|
||||
( _xmin, _ymin, _xmax, _ymin, _xmax, _ymax, _xmin, _ymax );
|
||||
_radius = radius;
|
||||
|
||||
// Inserts the vertices of the domain according to radius:
|
||||
if ( radius==0 )
|
||||
{ _using_domain = false;
|
||||
}
|
||||
else
|
||||
{ _using_domain = true;
|
||||
insert_polygon ( domain );
|
||||
}
|
||||
}
|
||||
|
||||
void SeDcdt::get_bounds ( float& xmin, float& xmax, float& ymin, float& ymax ) const
|
||||
{
|
||||
xmin=_xmin; xmax=_xmax; ymin=_ymin; ymax=_ymax;
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//=========================== insert polygon ====================================
|
||||
//================================================================================
|
||||
|
||||
int SeDcdt::insert_polygon ( const SrPolygon& pol )
|
||||
{
|
||||
int i, i1, id;
|
||||
SeVertex* v;
|
||||
SeFace* sface;
|
||||
|
||||
SR_TRACE1 ( "Inserting entry in the polygon set..." ); // put in _polygons
|
||||
id = _polygons.insert();
|
||||
SeDcdtInsPol& ip = *_polygons[id];
|
||||
ip.open = pol.open();
|
||||
|
||||
SR_TRACE1 ( "Inserting polygon points..." ); // insert vertices
|
||||
sface = _search_face();
|
||||
for ( i=0; i<pol.size(); i++ )
|
||||
{ v = _triangulator->insert_point ( pol[i].x, pol[i].y, sface );
|
||||
if ( !v ) sr_out.fatal_error ( "Search failure in _insert_polygon()." );
|
||||
ip.push() = (SeDcdtVertex*)v;
|
||||
sface = v->se()->fac();
|
||||
}
|
||||
|
||||
// should we keep here collinear vertices (which are not corners)?
|
||||
// they can be removed as Steiner vertices when removing another intersecting polygon
|
||||
|
||||
_cur_search_face=0; // Needed because edge constraint may call kef
|
||||
|
||||
SR_TRACE1 ( "Inserting polygon edges constraints..." ); // insert edges
|
||||
for ( i=0; i<ip.size(); i++ )
|
||||
{ i1 = (i+1)%ip.size();
|
||||
if ( i1==0 && ip.open ) break; // do not close the polygon
|
||||
if ( !_triangulator->insert_line_constraint ( ip[i], ip[i1], id ) )
|
||||
sr_out.fatal_error ( "Unable to insert constraint in _insert_polygon()." );
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
int SeDcdt::polygon_max_id () const
|
||||
{
|
||||
return _polygons.maxid();
|
||||
}
|
||||
|
||||
int SeDcdt::num_polygons () const
|
||||
{
|
||||
return _polygons.elements();
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//=========================== remove polygon ====================================
|
||||
//================================================================================
|
||||
|
||||
void SeDcdt::_find_intermediate_vertices_and_edges ( SeDcdtVertex* vini, int oid )
|
||||
{
|
||||
SeDcdtEdge *e;
|
||||
SeDcdtVertex *v;
|
||||
SeDcdtSymEdge *s, *si;
|
||||
|
||||
_mesh->begin_marking ();
|
||||
_varray.size(0); _earray.size(0); _stack.size(0);
|
||||
|
||||
// initialize stack with all constrained edges from v:
|
||||
_varray.push() = vini;
|
||||
_mesh->mark ( vini );
|
||||
s = si = vini->se();
|
||||
do { e = s->edg();
|
||||
if ( e->has_id(oid) )
|
||||
{ _stack.push() = s;
|
||||
_earray.push() = s;
|
||||
_mesh->mark ( e );
|
||||
}
|
||||
s = s->rot();
|
||||
} while ( s!=si );
|
||||
|
||||
// advances untill all edges are reached:
|
||||
while ( _stack.size() )
|
||||
{ s = si = _stack.pop()->nxt();
|
||||
v = s->vtx();
|
||||
if ( !_mesh->marked(v) )
|
||||
{ _varray.push() = v;
|
||||
_mesh->mark ( v );
|
||||
}
|
||||
do { e = s->edg();
|
||||
if ( !_mesh->marked(e) && e->has_id(oid) )
|
||||
{ _stack.push() = s;
|
||||
_earray.push() = s;
|
||||
_mesh->mark ( e );
|
||||
}
|
||||
s = s->rot();
|
||||
} while ( s!=si );
|
||||
}
|
||||
|
||||
_mesh->end_marking ();
|
||||
}
|
||||
|
||||
bool SeDcdt::_is_intersection_vertex
|
||||
( SeDcdtVertex* v, int id, SeDcdtVertex*& v1, SeDcdtVertex*& v2 )
|
||||
{
|
||||
SeDcdtSymEdge *si, *s;
|
||||
v1 = v2 = 0;
|
||||
|
||||
// at this point, it is guaranteed that only two constraints with index id
|
||||
// are adjacent to v, because there is a test that ensures that only two
|
||||
// constrained edges are incident to v in _remove_vertex_if_possible().
|
||||
// (however these two constrained edges may have more than one id)
|
||||
|
||||
si = s = v->se();
|
||||
do { if ( s->edg()->has_id(id) )
|
||||
{ if ( v1==0 ) v1=s->nxt()->vtx();
|
||||
else if ( v2==0 ) { v2=s->nxt()->vtx(); break; }
|
||||
}
|
||||
s = s->rot();
|
||||
} while ( s!=si );
|
||||
|
||||
if ( v1==0 || v2==0 ) return false; // v is an extremity of an open constraint
|
||||
|
||||
double d = sr_point_segment_dist ( v->p.x, v->p.y, v1->p.x, v1->p.y, v2->p.x, v2->p.y );
|
||||
return d<=_triangulator->epsilon()? true:false;
|
||||
}
|
||||
|
||||
void SeDcdt::_remove_vertex_if_possible ( SeDcdtVertex* v, const SrArray<int>& vids )
|
||||
{
|
||||
SR_TRACE4 ( "Try to remove vertex with ref="<<vids.size()<<"..." );
|
||||
|
||||
// Easier case, vertex is no more used:
|
||||
if ( vids.size()==0 ) // dangling vertex
|
||||
{ SR_TRACE4 ( "Removing dangling vertex" );
|
||||
_triangulator->remove_vertex(v);
|
||||
return;
|
||||
}
|
||||
|
||||
int i;
|
||||
int num_of_incident_constrained_edges=0;
|
||||
SeDcdtSymEdge *s;
|
||||
s = v->se();
|
||||
do { if ( s->edg()->is_constrained() ) num_of_incident_constrained_edges++;
|
||||
s = s->rot();
|
||||
} while ( s!=v->se() );
|
||||
|
||||
if ( num_of_incident_constrained_edges!=2 ) return;
|
||||
|
||||
// test if all polygons using the vertex no more need it:
|
||||
SeDcdtVertex *v1, *v2;
|
||||
SrArray<SeDcdtVertex*>& va = _varray2; // internal buffer
|
||||
va.size ( 0 );
|
||||
|
||||
// (note that we can have vids.size()>1 at this point)
|
||||
for ( i=0; i<vids.size(); i++ )
|
||||
{ if ( !_is_intersection_vertex(v,vids[i],v1,v2) )
|
||||
{ SR_TRACE4 ( "Not an intersection vertex!" );
|
||||
return;
|
||||
}
|
||||
va.push()=v1; va.push()=v2;
|
||||
}
|
||||
|
||||
SR_TRACE4 ( "Removing one intersection vertex..." );
|
||||
_triangulator->remove_vertex(v);
|
||||
|
||||
// and recover all deleted constraints:
|
||||
for ( i=0; i<vids.size(); i++ )
|
||||
{ _triangulator->insert_line_constraint ( va[i*2], va[i*2+1], vids[i] );
|
||||
}
|
||||
}
|
||||
|
||||
void SeDcdt::remove_polygon ( int polygonid )
|
||||
{
|
||||
int i;
|
||||
SR_TRACE4 ( "Entering remove_polygon..." );
|
||||
|
||||
if ( polygonid==0 )
|
||||
sr_out.fatal_error("Domain cannot be removed by SeDcdt::remove_polygon()");
|
||||
|
||||
if ( polygonid<0 || polygonid>_polygons.maxid() )
|
||||
sr_out.fatal_error("Invalid id sent to SeDcdt::remove_polygon()");
|
||||
|
||||
if ( !_polygons[polygonid] )
|
||||
sr_out.fatal_error("SeDcdt::remove_polygon(): polygon already removed");
|
||||
|
||||
// _search_face can be invalidated so make it unavailable:
|
||||
_cur_search_face = 0;
|
||||
|
||||
// Recuperate intermediate vertices inserted as Steiner points:
|
||||
SR_TRACE4 ( "Recuperating full polygon..." );
|
||||
_find_intermediate_vertices_and_edges ( _polygons[polygonid]->get(0), polygonid );
|
||||
SR_TRACE4 ( "polygon has "<<_varray.size()<<" vertices " << "and "<<_earray.size()<<" edges." );
|
||||
|
||||
// Remove all ids which are equal to polygonid in the edge constraints
|
||||
SR_TRACE4 ( "Updating edge constraints..." );
|
||||
for ( i=0; i<_earray.size(); i++ )
|
||||
{ _earray[i]->edg()->remove_id ( polygonid ); // remove all occurences
|
||||
}
|
||||
|
||||
SR_TRACE4 ( "Removing free vertices..." );
|
||||
for ( i=0; i<_varray.size(); i++ )
|
||||
{ _varray[i]->get_references ( _ibuffer ); //vobs
|
||||
SR_TRACE4 ( "Vertex "<<i<<": (" << _varray[i]->p << ")" );
|
||||
_remove_vertex_if_possible ( _varray[i], _ibuffer );
|
||||
}
|
||||
|
||||
_polygons.remove ( polygonid );
|
||||
SR_TRACE4 ( "Finished." );
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//============================= get polygon ======================================
|
||||
//================================================================================
|
||||
|
||||
void SeDcdt::get_polygon ( int polygonid, SrPolygon& polygon )
|
||||
{
|
||||
polygon.size(0);
|
||||
|
||||
if ( polygonid<0 || polygonid>_polygons.maxid() ) return;
|
||||
if ( !_polygons[polygonid] ) return;
|
||||
|
||||
SeDcdtInsPol& p = *_polygons[polygonid];
|
||||
|
||||
int i;
|
||||
polygon.size ( p.size() );
|
||||
polygon.open ( p.open? true:false );
|
||||
for ( i=0; i<p.size(); i++ )
|
||||
polygon[i] = p[i]->p;
|
||||
}
|
||||
|
||||
void SeDcdt::get_triangulated_polygon ( int polygonid, SrArray<SeDcdtVertex*>* vtxs, SrArray<SeDcdtEdge*>* edgs )
|
||||
{
|
||||
if ( polygonid<0 || polygonid>_polygons.maxid() ) return;
|
||||
if ( !_polygons[polygonid] ) return;
|
||||
|
||||
_find_intermediate_vertices_and_edges ( _polygons[polygonid]->get(0), polygonid );
|
||||
|
||||
if ( vtxs ) *vtxs = _varray;
|
||||
|
||||
if ( edgs ) // need to convert symedges to edges
|
||||
{ int i;
|
||||
edgs->size ( _earray.size() );
|
||||
for ( i=0; i<edgs->size(); i++ ) (*edgs)[i] = _earray[i]->edg();
|
||||
}
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//=========================== ray intersection ===================================
|
||||
//================================================================================
|
||||
|
||||
static bool _intersects ( double p1x, double p1y, double p2x, double p2y, SeDcdtSymEdge* s )
|
||||
{
|
||||
SeDcdtVertex* v1 = s->vtx();
|
||||
SeDcdtVertex* v2 = s->nxt()->vtx();
|
||||
return sr_segments_intersect ( p1x, p1y, p2x, p2y,
|
||||
v1->p.x, v1->p.y, v2->p.x, v2->p.y );
|
||||
}
|
||||
|
||||
void SeDcdt::ray_intersection ( float x1, float y1, float x2, float y2,
|
||||
SrArray<int>& polygons, int depth )
|
||||
{
|
||||
SeTriangulator::LocateResult res;
|
||||
SeBase *ses;
|
||||
SeDcdtSymEdge *s;
|
||||
|
||||
polygons.size(0);
|
||||
if ( depth==0 ) return;
|
||||
|
||||
double p1x=x1, p1y=y1, p2x=x2, p2y=y2;
|
||||
|
||||
res = _triangulator->locate_point ( _search_face(), p1x, p1y, ses );
|
||||
if ( res==SeTriangulator::NotFound )
|
||||
{ SR_TRACE7 ( "First point not found!" );
|
||||
return;
|
||||
}
|
||||
|
||||
s = (SeDcdtSymEdge*)ses;
|
||||
_cur_search_face = s->fac();
|
||||
|
||||
// Find first intersection with the first triangle:
|
||||
int i=0;
|
||||
while ( !_intersects(p1x,p1y,p2x,p2y,s) )
|
||||
{ s=s->nxt();
|
||||
if ( i++==3 )
|
||||
{ SR_TRACE7 ( "None intersections, ray in triangle!" );
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Continues until end:
|
||||
while ( true )
|
||||
{ if ( s->edg()->is_constrained() )
|
||||
{ //int r = rand() % e->ids_size;
|
||||
SR_TRACE7 ( "found one!" );
|
||||
SrArray<int>& a = s->edg()->ids;
|
||||
for ( i=0; i<a.size(); i++ )
|
||||
if ( polygons.size()<depth ) polygons.push()=a[i];
|
||||
if ( depth>0 && polygons.size()>=depth )
|
||||
{ SR_TRACE7 ( "depth reached!" ); return; }
|
||||
}
|
||||
s = s->sym();
|
||||
if ( _intersects(p1x,p1y,p2x,p2y,s->nxt()) ) s=s->nxt();
|
||||
else
|
||||
if ( _intersects(p1x,p1y,p2x,p2y,s->pri()) ) s=s->pri();
|
||||
else
|
||||
{ SR_TRACE7 ( "Finished Ray!" ); return; }
|
||||
}
|
||||
}
|
||||
|
||||
void SeDcdt::_add_contour ( SeDcdtSymEdge* s, SrPolygon& vertices, SrArray<int>& pindices )
|
||||
{
|
||||
SeDcdtSymEdge* si=s;
|
||||
pindices.push() = vertices.size();
|
||||
|
||||
SR_TRACE9 ( "Begin contour: "<<pindices.top() );
|
||||
do { vertices.push() = s->vtx()->p;
|
||||
_mesh->mark ( s->edg() );
|
||||
do { s=s->rot(); } while ( !s->edg()->is_constrained() );
|
||||
s = s->sym();
|
||||
} while ( s!=si );
|
||||
|
||||
pindices.push() = vertices.size()-1;
|
||||
SR_TRACE9 ( "End contour: "<<pindices.top() );
|
||||
}
|
||||
|
||||
SeDcdtSymEdge* SeDcdt::_find_one_obstacle ()
|
||||
{
|
||||
SeDcdtSymEdge *s;
|
||||
|
||||
while ( 1 )
|
||||
{ if ( _earray.empty() ) break;
|
||||
|
||||
s = _earray.pop();
|
||||
if ( _mesh->marked(s->edg()) ) continue;
|
||||
if ( s->edg()->is_constrained() ) return s;
|
||||
_mesh->mark(s->edg());
|
||||
|
||||
s = s->sym()->nxt();
|
||||
if ( !_mesh->marked(s->edg()) ) { _earray.push()=s; }
|
||||
s = s->nxt();
|
||||
if ( !_mesh->marked(s->edg()) ) { _earray.push()=s; }
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void SeDcdt::extract_contours ( SrPolygon& vertices, SrArray<int>& pindices, float x, float y )
|
||||
{
|
||||
SeTriangulator::LocateResult res;
|
||||
SeBase *ses;
|
||||
|
||||
vertices.size(0);
|
||||
pindices.size(0);
|
||||
|
||||
SR_TRACE9 ( "Begin Extract" );
|
||||
|
||||
res = _triangulator->locate_point ( _search_face(), x, y, ses );
|
||||
if ( res==SeTriangulator::NotFound ) return;
|
||||
|
||||
_mesh->begin_marking();
|
||||
_earray.size(0);
|
||||
SeDcdtSymEdge *s, *si;
|
||||
s = si = (SeDcdtSymEdge*)ses;
|
||||
do { _earray.push() = s;
|
||||
s = s->nxt();
|
||||
} while ( s!=si );
|
||||
|
||||
while (1)
|
||||
{ s = _find_one_obstacle ();
|
||||
if ( !s ) break;
|
||||
_add_contour ( s, vertices, pindices );
|
||||
}
|
||||
|
||||
_mesh->end_marking();
|
||||
SR_TRACE9 ( "End Extract" );
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//=========================== inside polygon ====================================
|
||||
//================================================================================
|
||||
|
||||
static int interhoriz ( float px, float py, float p1x, float p1y, float p2x, float p2y )
|
||||
{
|
||||
if ( p1y>p2y ) { float tmp; SR_SWAP(p1x,p2x); SR_SWAP(p1y,p2y); } // swap
|
||||
if ( p1y>=py ) return false; // not intercepting
|
||||
if ( p2y<py ) return false; // not intercepting or = max
|
||||
float x2 = p1x + (py-p1y) * (p2x-p1x) / (p2y-p1y);
|
||||
return (px<x2)? true:false;
|
||||
}
|
||||
|
||||
int SeDcdt::inside_polygon ( float x, float y , SrArray<int>* allpolys )
|
||||
{
|
||||
if ( _polygons.elements()<=1 ) return -1; // if there is only the domain, return.
|
||||
|
||||
int cont=0, i, size;
|
||||
SeDcdtInsPol* pol;
|
||||
SeDcdtVertex *v1, *v2;
|
||||
|
||||
if ( allpolys ) allpolys->size(0);
|
||||
|
||||
int polid=0;
|
||||
if ( _using_domain ) polid++;
|
||||
for ( ; polid<=_polygons.maxid(); polid++ )
|
||||
{ pol = _polygons[polid];
|
||||
if ( !pol ) continue;
|
||||
|
||||
size = pol->size();
|
||||
for ( i=0; i<size; i++ )
|
||||
{ v1 = pol->get(i);
|
||||
v2 = pol->get( (i+1)%size );
|
||||
cont ^= interhoriz ( x, y, v1->p.x, v1->p.y, v2->p.x, v2->p.y );
|
||||
}
|
||||
if (cont)
|
||||
{ if ( allpolys ) allpolys->push()=polid;
|
||||
else return polid;
|
||||
}
|
||||
}
|
||||
|
||||
if ( allpolys ) if ( allpolys->size()>0 ) return allpolys->get(0);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int SeDcdt::pick_polygon ( float x, float y )
|
||||
{
|
||||
if ( _polygons.elements()==0 ) return -1;
|
||||
if ( _using_domain && _polygons.elements()==1 ) return -1; // if there is only the domain, return.
|
||||
|
||||
SeTriangulator::LocateResult res;
|
||||
SeBase *sse;
|
||||
|
||||
res = _triangulator->locate_point ( _search_face(), x, y, sse );
|
||||
if ( res==SeTriangulator::NotFound ) return -1;
|
||||
|
||||
SeDcdtSymEdge *s = (SeDcdtSymEdge*)sse;
|
||||
SrPnt2 pt(x,y);
|
||||
float d1 = dist2 ( pt, s->vtx()->p );
|
||||
float d2 = dist2 ( pt, s->nxt()->vtx()->p );
|
||||
float d3 = dist2 ( pt, s->nxt()->nxt()->vtx()->p );
|
||||
if ( d2<=d1 && d2<=d3 ) s=s->nxt();
|
||||
if ( d3<=d1 && d3<=d2 ) s=s->nxt()->nxt();
|
||||
|
||||
SeDcdtSymEdge* k=s;
|
||||
do { SrArray<int>& ids = k->edg()->ids;
|
||||
if ( ids.size()>0 ) { if ( !_using_domain || ids[0]>0 ) return ids[0]; } // 0 is the domain
|
||||
k=k->rot();
|
||||
} while ( k!=s );
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
//================================================================================
|
||||
//=============================== search path ======================================
|
||||
//================================================================================
|
||||
|
||||
bool SeDcdt::search_path ( float x1, float y1, float x2, float y2,
|
||||
const SeDcdtFace* iniface, bool vistest )
|
||||
{
|
||||
// fast security test to ensure at least that points are not outside the border limits:
|
||||
if ( x1<_xmin || x1>_xmax || x2<_xmin || x2>_xmax ) return false;
|
||||
|
||||
if ( !iniface ) iniface = _search_face();
|
||||
|
||||
bool found = _triangulator->search_path ( x1, y1, x2, y2, iniface, vistest );
|
||||
|
||||
// to optimize searching for next queries around the same point,
|
||||
// set the next starting search face to the first channel face:
|
||||
if ( _triangulator->get_channel_interior_edges().size()>0 )
|
||||
_cur_search_face = (SeDcdtFace*)_triangulator->get_channel_interior_edges()[0]->fac();
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
//============================ End of File ===============================
|
||||
|
||||
493
source/dcdt/se/se_dcdt.h
Normal file
493
source/dcdt/se/se_dcdt.h
Normal file
@ -0,0 +1,493 @@
|
||||
|
||||
# ifndef SE_DCDT_H
|
||||
# define SE_DCDT_H
|
||||
|
||||
/** \file se_dcdt.h
|
||||
* Dynamic Constrained Delaunay Triangulaiton
|
||||
*/
|
||||
|
||||
# include "sr_set.h"
|
||||
# include "sr_vec2.h"
|
||||
# include "sr_polygon.h"
|
||||
|
||||
# include "se_triangulator.h"
|
||||
|
||||
//DJD: Including definition of abstraction and search node classes {
|
||||
|
||||
//#define EXPERIMENT
|
||||
#define DEBUG
|
||||
#define FIRST_PATH_FAST
|
||||
|
||||
#include "Abstraction.h"
|
||||
#include "SearchNode.h"
|
||||
#if defined EXPERIMENT
|
||||
#include "Experiments.h"
|
||||
#endif
|
||||
|
||||
//Including definition of abstraction and search node classes }
|
||||
|
||||
//============================ DCDT Mesh definitions ==================================
|
||||
|
||||
class SeDcdtVertex; // contains 2d coordinates
|
||||
class SeDcdtEdge; // contains constraints ids
|
||||
|
||||
//DJD: changing definition of SeDcdtFace {
|
||||
|
||||
//class SeDcdtFace; // a default element
|
||||
|
||||
//changing definition of SeDcdtFace }
|
||||
|
||||
typedef Se < SeDcdtVertex, SeDcdtEdge, SeDcdtFace > SeDcdtSymEdge;
|
||||
typedef SeMesh < SeDcdtVertex, SeDcdtEdge, SeDcdtFace > SeDcdtMesh;
|
||||
|
||||
//DJD: defining SeLinkFace {
|
||||
|
||||
//SE_DEFINE_DEFAULT_ELEMENT(SeDcdtFace,SeDcdtSymEdge);
|
||||
|
||||
template <class T>
|
||||
class SeLinkFace : public SeElement
|
||||
{
|
||||
public:
|
||||
//the number of new faces that haven't been dealt with
|
||||
static int Faces() {return processing.size();}
|
||||
//retrieve a particular unprocessed face
|
||||
static SeLinkFace<T> *Face(int n) {return ((n < 0) || (n >= processing.size())) ? NULL : processing[n];}
|
||||
//clear unprocessed face list, increment the global update
|
||||
static void Clear() {while (!processing.empty()) processing.pop(); current++;}
|
||||
//link to external information
|
||||
T *link;
|
||||
SE_ELEMENT_CASTED_METHODS(SeLinkFace, SeDcdtSymEdge);
|
||||
//default constructor - set the link to null and initialize the face
|
||||
SeLinkFace() : link (NULL) {Initialize();}
|
||||
//copy constructor - initialize the link to that of the other face, and initialize it
|
||||
SeLinkFace(const SeLinkFace& f) : link (f.link) {Initialize();}
|
||||
//constructor - initialize the link to the value passed and initialize the face
|
||||
SeLinkFace(T *l) : link (l) {Initialize();}
|
||||
//destructor - deletes a non-null link, and nulls its entry in the unprocessed list if it's in there
|
||||
~SeLinkFace() {if (link != NULL) delete link; if (update == current) processing[index] = NULL;}
|
||||
friend SrOutput& operator<<(SrOutput& out, const SeLinkFace<T>& f) {return out;}
|
||||
friend SrInput& operator>>(SrInput& inp, SeLinkFace<T>& f) {return inp;}
|
||||
friend int sr_compare(const SeLinkFace<T>* f1, const SeLinkFace<T>* f2) {return 0;}
|
||||
protected:
|
||||
//the index of this face in the unprocessed list
|
||||
int index;
|
||||
//the update when this face was inserted in the unprocessed list
|
||||
int update;
|
||||
//initializes the face - puts it in the unprocessed list and sets its index and update
|
||||
void Initialize() {index = processing.size(); processing.push() = this; update = current;}
|
||||
//the unprocessed list
|
||||
static SrArray<SeLinkFace<T> *> processing;
|
||||
//the global update
|
||||
static int current;
|
||||
};
|
||||
|
||||
//defining SeLinkFace }
|
||||
|
||||
class SeDcdtVertex : public SeElement
|
||||
{ public :
|
||||
SrPnt2 p; // 2d coordinates
|
||||
public :
|
||||
SE_ELEMENT_CASTED_METHODS(SeDcdtVertex,SeDcdtSymEdge);
|
||||
SeDcdtVertex () {}
|
||||
SeDcdtVertex ( const SeDcdtVertex& v ) : p(v.p) {}
|
||||
SeDcdtVertex ( const SrPnt2& pnt ) : p(pnt) {}
|
||||
void set ( float x, float y ) { p.x=x; p.y=y; }
|
||||
void get_references ( SrArray<int>& ids ); // get all constr edges referencing this vertex
|
||||
friend SrOutput& operator<< ( SrOutput& out, const SeDcdtVertex& v );
|
||||
friend SrInput& operator>> ( SrInput& inp, SeDcdtVertex& v );
|
||||
friend int sr_compare ( const SeDcdtVertex* v1, const SeDcdtVertex* v2 ) { return 0; } // not used
|
||||
};
|
||||
|
||||
class SeDcdtEdge : public SeElement
|
||||
{ public :
|
||||
SrArray<int> ids; // ids of all constraints sharing this edge
|
||||
public :
|
||||
SE_ELEMENT_CASTED_METHODS(SeDcdtEdge,SeDcdtSymEdge);
|
||||
SeDcdtEdge () { }
|
||||
SeDcdtEdge ( const SeDcdtEdge& e ) { ids=e.ids; }
|
||||
bool is_constrained() const { return ids.size()>0? true:false; }
|
||||
void set_unconstrained () { ids.size(0); }
|
||||
bool has_id ( int id ) const;
|
||||
bool has_other_id_than ( int id ) const;
|
||||
bool remove_id ( int id );
|
||||
void add_constraints ( const SrArray<int>& ids );
|
||||
friend SrOutput& operator<< ( SrOutput& out, const SeDcdtEdge& e );
|
||||
friend SrInput& operator>> ( SrInput& inp, SeDcdtEdge& e );
|
||||
friend int sr_compare ( const SeDcdtEdge* e1, const SeDcdtEdge* e2 ) { return 0; } // not used
|
||||
};
|
||||
|
||||
//================================ DCDT internal classes =====================================
|
||||
|
||||
class SeDcdtTriManager: public SeTriangulatorManager
|
||||
{ public :
|
||||
virtual void get_vertex_coordinates ( const SeVertex* v, double& x, double & y );
|
||||
virtual void set_vertex_coordinates ( SeVertex* v, double x, double y );
|
||||
virtual bool is_constrained ( SeEdge* e );
|
||||
virtual void set_unconstrained ( SeEdge* e );
|
||||
virtual void get_constraints ( SeEdge* e, SrArray<int>& ids );
|
||||
virtual void add_constraints ( SeEdge* e, const SrArray<int>& ids );
|
||||
};
|
||||
|
||||
class SeDcdtInsPol: public SrArray<SeDcdtVertex*> // keeps one "inserted polygon"
|
||||
{ public :
|
||||
char open;
|
||||
public :
|
||||
SeDcdtInsPol () { open=0; }
|
||||
SeDcdtInsPol ( const SeDcdtInsPol& ob ) : SrArray<SeDcdtVertex*>((const SrArray<SeDcdtVertex*>&)ob) { open=0; }
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SeDcdtInsPol& ob );
|
||||
friend SrInput& operator>> ( SrInput& in, SeDcdtInsPol& ob );
|
||||
friend int sr_compare ( const SeDcdtInsPol* c1, const SeDcdtInsPol* c2 ) { return 0; } // not used
|
||||
};
|
||||
|
||||
//=================================== DCDT class ========================================
|
||||
|
||||
/*! Mantains a dynamic constrained Delaunay triangulation of given polygons
|
||||
for the purpose of path planning and other queries. */
|
||||
class SeDcdt
|
||||
{ private :
|
||||
SeDcdtMesh* _mesh; // the mesh
|
||||
SeTriangulator* _triangulator; // the used triangulator
|
||||
SeDcdtSymEdge* _first_symedge; // symedge at the border, but not at the back face
|
||||
SeDcdtFace* _cur_search_face; // a face near the last change in the mesh, 0 if not valid
|
||||
SrSet<SeDcdtInsPol> _polygons; // inserted polygons, polygon 0 is always the domain
|
||||
SrArray<SeDcdtVertex*> _varray; // internal buffer
|
||||
SrArray<SeDcdtSymEdge*> _earray; // internal buffer
|
||||
SrArray<SeDcdtSymEdge*> _stack; // internal buffer
|
||||
SrArray<SeDcdtVertex*> _varray2; // internal buffer
|
||||
SrArray<int> _ibuffer;
|
||||
float _radius;
|
||||
bool _using_domain;
|
||||
float _xmin, _xmax, _ymin, _ymax;
|
||||
|
||||
public :
|
||||
/*! Default constructor */
|
||||
SeDcdt ();
|
||||
|
||||
/*! Destructor */
|
||||
~SeDcdt ();
|
||||
|
||||
/*! Returns a pointer to the internal maintained mesh. The user can then
|
||||
use low level methods of SeMesh for additional computation. However it
|
||||
is the user responsability to not conflict with SeDcdt methods. */
|
||||
const SeDcdtMesh* mesh() const { return _mesh; }
|
||||
|
||||
/*! Put in the given arrays the coordinates of the constrained and
|
||||
unconstrained edges endpoints. Each two consecutive points in the
|
||||
returned arrays give the first and end points of one dge.
|
||||
Parameters can be null indicating that no data of that type is requested.
|
||||
The two parameters are also allowed to point to the same array */
|
||||
void get_mesh_edges ( SrArray<SrPnt2>* constr, SrArray<SrPnt2>* unconstr );
|
||||
|
||||
/*! Save the current dcdt by saving the list of all inserted obstacles.
|
||||
Note that the polygons ids are preserved. */
|
||||
bool save ( SrOutput& out );
|
||||
|
||||
/*! Destructs the current map and loads a new one */
|
||||
bool load ( SrInput& inp );
|
||||
|
||||
/*! Initializes the triangulation with a domain polygon.
|
||||
The domain is considered to be the constraint polygon with id 0; and can
|
||||
be retrieved again by calling get_polygon(0).
|
||||
An external box is automatically computed as an expanded bounding box of
|
||||
the domain, where the expansion vector length is 1/5 of the bounding box sides.
|
||||
This external box defines the border of the triangulation and its coordinates
|
||||
can be retrieved with get_bounds().
|
||||
Note that all polygons inserted afterwards must be inside the external box.
|
||||
Parameter radius can be used, for instance, to have a security margin in order
|
||||
to allow growing polygons without intersecting with the external box.
|
||||
If parameter radius is >0, it is used as minimum length for the
|
||||
expansion vector used to compute the external box.
|
||||
Special Case: If radius is 0, the domain polygon is not inserted, and the
|
||||
triangulation is initialized with border equal to the bounding box of domain.
|
||||
Parameter epsilon is simply passed to the triangulator. */
|
||||
void init ( const SrPolygon& domain, float epsilon, float radius=-1 );
|
||||
|
||||
/*! Internally, the border is generated containing the domain polygon.
|
||||
This method allows to retrieve the coordinates of the border rectangle. */
|
||||
void get_bounds ( float& xmin, float& xmax, float& ymin, float& ymax ) const;
|
||||
|
||||
/*! Inserts a polygon as a constraint in the CDT, returning its id.
|
||||
In case of error, -1 is returned. Polygons can be open.
|
||||
The returned id can be used to remove the polygon later on.
|
||||
All kinds of intersections and overllapings are handled.
|
||||
Collinear vertices are inserted. If not desired, method
|
||||
SrPolygon::remove_collinear_vertices() should be called prior insertion. */
|
||||
int insert_polygon ( const SrPolygon& polygon );
|
||||
|
||||
/*! Returns the max id currently being used. Note that a SrSet controls
|
||||
the ids, so that the max id may not correspond to the number of
|
||||
polygons inserted; ids values are not changed with polygon removal */
|
||||
int polygon_max_id () const;
|
||||
|
||||
/*! Returns the number of inserted polygons, including the domain (if inserted). */
|
||||
int num_polygons () const;
|
||||
|
||||
/*! Remove a previoulsy inserted polygon. false may be returned if the id is
|
||||
not valid or if some internal error occurs. The domain cannot be removed with
|
||||
this method. Steiner points may stay as part of other polygons if the
|
||||
triangulation is in conforming mode. */
|
||||
void remove_polygon ( int polygonid );
|
||||
|
||||
/*! Retrieves the original vertices of the given polygon (without collinear vertices).
|
||||
If the returned polygon is empty, it means that polygonid is invalid. */
|
||||
void get_polygon ( int polygonid, SrPolygon& polygon );
|
||||
|
||||
/*! Returns the vertices and edges used by the polygon in the triangulation.
|
||||
Elements may be out of order, specially when they have intersections.
|
||||
If an argument is a null pointer, nothing is done with it. */
|
||||
void get_triangulated_polygon ( int polygonid, SrArray<SeDcdtVertex*>* vtxs, SrArray<SeDcdtEdge*>* edgs );
|
||||
|
||||
/*! Returns a list with the ids of the polygons having some edge traversed by the segment
|
||||
[(x1,y1),(x2,y2)]. The length of the returned array will not be more than depth,
|
||||
corresponding to 'depth' edges being crossed. Note: the id of a polygon will
|
||||
appear both when the ray enters the polygon and when it leaves the polygon.
|
||||
If depth is <0, no depth control is used.
|
||||
This routine can also be used to fastly determine if a point is inside a polygon
|
||||
by looking if the number of intersections is odd or even. */
|
||||
void ray_intersection ( float x1, float y1, float x2, float y2, SrArray<int>& polygons, int depth );
|
||||
|
||||
/*! Returns all polygons describing the contours of an "eating virus" starting at x,y.
|
||||
Array pindices contains, for each contour, the starting and ending vertex index,
|
||||
which are sequentially stored in array vertices. */
|
||||
void extract_contours ( SrPolygon& vertices, SrArray<int>& pindices, float x, float y );
|
||||
|
||||
/*! Returns the id of the first found polygon containing the given point (x,y),
|
||||
or -1 if no polygons are found. The domain polygon, if used in init(), will not
|
||||
be considered. The optional parameter allpolys can be sent to return all polygons
|
||||
containing the point, and only the first one.
|
||||
Note: this method does a linear search over each polygon, alternativelly, the
|
||||
ray_intersection() method might also be used to detect polygon containement. */
|
||||
int inside_polygon ( float x, float y, SrArray<int>* allpolys=0 );
|
||||
|
||||
/*! Returns the id of one polygon close to the given point (x,y), or -1 otherwise.
|
||||
This method locates the point (x,y) in the triangulation and then takes the
|
||||
nearest polygon touching that triangle.
|
||||
The domain polygon, if used in init(), will not be considered. */
|
||||
int pick_polygon ( float x, float y );
|
||||
|
||||
/*! Search for the channel connecting x1,y1 and x2,y2.
|
||||
It simply calls SeTriangulator::search_path(), however here parameter iniface is optional */
|
||||
bool search_path ( float x1, float y1, float x2, float y2,
|
||||
const SeDcdtFace* iniface=0, bool vistest=false );
|
||||
|
||||
/*! Returns a reference to the list with the interior edges of the last channel
|
||||
determined by a sussesfull call to search_path */
|
||||
const SrArray<SeBase*>& get_channel_interior_edges () const
|
||||
{ return _triangulator->get_channel_interior_edges(); }
|
||||
|
||||
/*! Returns a polygon describing the current channel, and thus, method find_path must
|
||||
be succesfully called before to determine the channel to consider. */
|
||||
void get_channel_boundary ( SrPolygon& channel ) { _triangulator->get_channel_boundary(channel); }
|
||||
|
||||
/*! Returns the canonical path, which is the path passing through the midpoint of
|
||||
the channel interior edges. Method search_path must be succesfully called before
|
||||
in order to determine the channel. The path is returned as an open polygon. */
|
||||
void get_canonical_path ( SrPolygon& path ) { _triangulator->get_canonical_path(path); }
|
||||
|
||||
/*! Returns the shortest path inside the current channel using the funnel algorithm. */
|
||||
void get_shortest_path ( SrPolygon& path ) { _triangulator->get_shortest_path(path); }
|
||||
|
||||
//DJD: prototypes for functions used in abstraction {
|
||||
|
||||
public:
|
||||
#if defined DEBUG
|
||||
//the triangle on which the mouse pointer currently resides
|
||||
SeDcdtFace *cursor;
|
||||
#endif
|
||||
//adds abstraction information to the triangulation
|
||||
void Abstract();
|
||||
//deletes all abstraction information from the triangulation
|
||||
void DeleteAbstraction();
|
||||
private:
|
||||
//the "outside" face of the triangulation
|
||||
SeDcdtFace *outside;
|
||||
//abstract an acyclic component of the triangulation graph
|
||||
void TreeAbstract(SeDcdtFace *first, int component);
|
||||
//collapse an acyclic portion into the degree-2 root given
|
||||
void TreeCollapse(SeDcdtFace *root, SeDcdtFace *currentFace, int component);
|
||||
|
||||
//prototypes for functions used in abstraction }
|
||||
|
||||
//DJD: prototypes for functions used in width calculation {
|
||||
|
||||
private:
|
||||
//calculates all widths through a triangle
|
||||
void CalculateWidths(SeDcdtFace *face);
|
||||
//calculates a particular width through a triangle
|
||||
float TriangleWidth(SeBase *s);
|
||||
//search across an edge for the width through the triangle
|
||||
float SearchWidth(float x, float y, SeBase *s, float CurrentWidth);
|
||||
//checks if the position given in the face given is at least r from any obstacle
|
||||
bool ValidPosition(float x, float y, SeDcdtFace *face, float r);
|
||||
//determines if angles (x, y)-(x1, y1)-(x2, y2) and (x, y)-(x2, y2)-(x1, y1) are accute
|
||||
bool Consider(float x, float y, float x1, float y1, float x2, float y2);
|
||||
//determines the minimum distance between the point (x, y) and the line going through (x1, y1)-(x2, y2)
|
||||
float PointLineDistance(float x, float y, float x1, float y1, float x2, float y2);
|
||||
|
||||
//prototypes for functions used in width calculation }
|
||||
|
||||
//DJD: protptypes for functions used in point location {
|
||||
|
||||
public:
|
||||
//perform initialization for sector-based point location on the unprocessed triangles
|
||||
void InitializeSectors();
|
||||
//checks if the given point is inside the given triangle
|
||||
bool InTriangle(SeDcdtFace *face, float x, float y);
|
||||
//use sector-based point location to find the face in which the given point resides
|
||||
SeTriangulator::LocateResult LocatePoint(float x, float y, SeBase* &result);
|
||||
//use regular point location to perform the above
|
||||
SeTriangulator::LocateResult LocatePointOld(float x, float y, SeBase* &result);
|
||||
//generate a random valid x value for testing point location
|
||||
float RandomX();
|
||||
//generate a random valid y value for testing point location
|
||||
float RandomY();
|
||||
private:
|
||||
//the number of sectors in the vertical dimension
|
||||
const static int ySectors = 10;
|
||||
//the number of sectors in the horizontal dimension
|
||||
const static int xSectors = 10;
|
||||
//the width of a single sector
|
||||
float sectorWidth;
|
||||
//the height of a single sector
|
||||
float sectorHeight;
|
||||
//the array of sectors
|
||||
SeDcdtFace *sectors[ySectors][xSectors];
|
||||
//return the maximum of the 3 numbers passed
|
||||
float Max(float a, float b, float c);
|
||||
//return the maximum of the 3 numbers passed
|
||||
float Min(float a, float b, float c);
|
||||
//return a random value between those given
|
||||
float RandomBetween(float min, float max);
|
||||
|
||||
//protptypes for functions used in point location }
|
||||
|
||||
//DJD: prototypes for functions used in abstracted space searching {
|
||||
|
||||
public:
|
||||
#if defined EXPERIMENT
|
||||
//find given path with TRA* and return experimental data
|
||||
bool SearchPathFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData);
|
||||
//find given path with TA* and return experimental data
|
||||
bool SearchPathBaseFast(float x1, float y1, float x2, float y2, float r, Data *data, int numData);
|
||||
#else
|
||||
//find given path with TRA*
|
||||
bool SearchPathFast(float x1, float y1, float x2, float y2, float r);
|
||||
//find given path with TA*
|
||||
bool SearchPathBaseFast(float x1, float y1, float x2, float y2, float r);
|
||||
#endif
|
||||
//get the shortest path between the given points within the given channel of triangles
|
||||
float GetShortestPath(SrPolygon& path, SrArray<SeBase *> Channel, float x1, float y1, float x2, float y2, float r);
|
||||
//return the polygon around the current channel
|
||||
SrPolygon GetChannelBoundary();
|
||||
//return the current path
|
||||
SrPolygon &GetPath();
|
||||
private:
|
||||
#if defined EXPERIMENT
|
||||
//search for a path in an acyclic portion of the triangulation graph and count the triangles searched
|
||||
bool Degree1Path(SrArray<SeBase *>& path, float x1, float y1, SeDcdtFace *startFace,
|
||||
float x2, float y2, SeDcdtFace *goalFace, float r, int &nodes);
|
||||
#else
|
||||
//search for a path in an acyclic portion of the triangulation graph
|
||||
bool Degree1Path(SrArray<SeBase *>& path, float x1, float y1, SeDcdtFace *startFace,
|
||||
float x2, float y2, SeDcdtFace *goalFace, float r);
|
||||
#endif
|
||||
//search for a path along a degree-2 corridor
|
||||
bool Degree2Path(SrArray<SeBase *>& path, SeDcdtFace *startFace, SeDcdtFace *nextFace,
|
||||
SeDcdtFace *goalFace, float r);
|
||||
//search for a path along a degree-2 corridor, crossing a degree-3 node
|
||||
bool FollowLoop(SrArray<SeBase *>& path, SeDcdtFace *startFace, SeDcdtFace *nextFace,
|
||||
SeDcdtFace *goalFace, SeDcdtFace *degree3, float r);
|
||||
//walk from one face to a given adjacent face, in a direction specified
|
||||
bool WalkBetween(SrArray<SeBase *>& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction = INVALID);
|
||||
//determine if an object of given radius can cross the middle triangle between the adjacent ones given
|
||||
bool CanCross(SeBase *first, SeBase *second, SeBase *third, float r);
|
||||
bool CanCross(SeDcdtFace *middle, SeBase *end1, SeBase *end2, float r);
|
||||
//checks if a unit of radius r can enter the path from the start and goal
|
||||
bool ValidEndpoints(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r);
|
||||
//checks if a unit has to cross the middle of the triangle to enter the path from the point given
|
||||
bool ValidEndpoint(SeBase *s, float x, float y, bool left);
|
||||
//determines if a unit can fit through the middle of the given triangle to get to the path
|
||||
bool ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r);
|
||||
//checks if a given path is valid for a unit of radius r
|
||||
bool ValidPath(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r);
|
||||
//returns the closest point in a triangle to a given point for a unit of given radius
|
||||
SrPnt2 ClosestPointTo(SeDcdtFace *face, float x, float y, float r);
|
||||
//given a search node, construct the channel it represents
|
||||
SrArray<SeBase *> ConstructBaseChannel(SearchNode *goal);
|
||||
//given a search node, construct a channel, filling in degree-1 and 2 nodes between those searched
|
||||
void ConstructBasePath(SrArray<SeBase *> &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction = INVALID);
|
||||
//the current channel
|
||||
SrArray<SeBase *> currentChannel;
|
||||
//the current path
|
||||
SrPolygon currentPath;
|
||||
//the current start position
|
||||
SrPnt2 startPoint;
|
||||
//the current goal position
|
||||
SrPnt2 goalPoint;
|
||||
|
||||
//prototypes for functions used in abstracted space searching }
|
||||
|
||||
//DJD: prototypes for utility functions {
|
||||
|
||||
public:
|
||||
//retrieves the vertices and whether the edges are constrained, for a given triangle
|
||||
void TriangleInfo(SeDcdtFace *face, float &x1, float &y1, bool &e1,
|
||||
float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3);
|
||||
void TriangleInfo(SeBase *s, float &x1, float &y1, bool &e1,
|
||||
float &x2, float &y2, bool &e2, float &x3, float &y3, bool &e3);
|
||||
//revireves the vertices for a given triangle
|
||||
void TriangleVertices(SeDcdtFace *face, float &x1, float &y1,
|
||||
float &x2, float &y2, float &x3, float &y3);
|
||||
void TriangleVertices(SeBase *s, float &x1, float &y1,
|
||||
float &x2, float &y2, float &x3, float &y3);
|
||||
//retrieves whether the edges are constrained, for a given triangle
|
||||
void TriangleEdges(SeDcdtFace *face, bool &e1, bool &e2, bool &e3);
|
||||
void TriangleEdges(SeBase *s, bool &e1, bool &e2, bool &e3);
|
||||
//returns the midpoint of a given triangle
|
||||
void TriangleMidpoint(SeDcdtFace *face, float &x, float &y);
|
||||
SrPnt2 TriangleMidpoint(SeDcdtFace *face);
|
||||
//returns whether a given edge is either constrained, or borders the "outside" polygon
|
||||
bool Blocked(SeBase *s);
|
||||
//returns the degree of a triangle's node (UNABSTRACTED if it has not been mapped)
|
||||
int Degree(SeBase *s);
|
||||
int Degree(SeFace *face);
|
||||
private:
|
||||
//returns whether the given angle is accute
|
||||
bool IsAccute(float theta);
|
||||
//returns whether the given angle is obtuse
|
||||
bool IsObtuse(float theta);
|
||||
//returns the length of the line segment (x1, y1)-(x2, y2)
|
||||
float Length(float x1, float y1, float x2, float y2);
|
||||
//returns the smaller of the two values passed
|
||||
float Minimum(float a, float b);
|
||||
//returns the larger of the two values passed
|
||||
float Maximum(float a, float b);
|
||||
//determines the angle between (x1, y1)-(x2, y2)-(x3, y3)
|
||||
float AngleBetween(float x1, float y1, float x2, float y2, float x3, float y3);
|
||||
//returns 0 if the points are colinear, <0 if they are in clockwise order, and >0 for counterclockwise
|
||||
float Orientation(float x1, float y1, float x2, float y2, float x3, float y3);
|
||||
//returns the minimum distance between the line segments (A1x, A1y)-(A2x, A2y) and (B1x, B1y)-(B2x, B2y)
|
||||
float SegmentDistance(float A1x, float A1y, float A2x, float A2y, float B1x, float B1y, float B2x, float B2y);
|
||||
//returns the minimum distance between the line segments (ls1x, ls1y)-(ls2x, ls2y) and (ll1x, ll1y)-(ll2x, ll2y),
|
||||
//at least r away from the segments' end points
|
||||
float SegmentDistance(float ls1x, float ls1y, float ls2x, float ls2y, float ll1x, float ll1y, float ll2x, float ll2y, float r);
|
||||
//returns the minimum distance between point (x, y) and segment (x1, y1)-(x2, y2)
|
||||
float PointSegmentDistance(float x, float y, float x1, float y1, float x2, float y2);
|
||||
//returns the closest point on the segment (x1, y1)-(x2, y2) to point (x, y),
|
||||
//at least r away from th segments' midpoints
|
||||
SrPnt2 ClosestPointOn(float x1, float y1, float x2, float y2, float x, float y, float r);
|
||||
|
||||
//prototypes for utility functions }
|
||||
|
||||
private :
|
||||
SeDcdtFace* _search_face() { return _cur_search_face? _cur_search_face:_first_symedge->fac(); }
|
||||
void _find_intermediate_vertices_and_edges ( SeDcdtVertex* vini, int oid );
|
||||
bool _is_intersection_vertex ( SeDcdtVertex* v, int oid, SeDcdtVertex*& v1, SeDcdtVertex*& v2 );
|
||||
void _remove_vertex_if_possible ( SeDcdtVertex* v, const SrArray<int>& vids );
|
||||
void _add_contour ( SeDcdtSymEdge* s, SrPolygon& vertices, SrArray<int>& pindices );
|
||||
SeDcdtSymEdge* _find_one_obstacle ();
|
||||
};
|
||||
|
||||
//================================== End of File =========================================
|
||||
|
||||
# endif // SE_DCDT_H
|
||||
148
source/dcdt/se/se_dcdt_mesh.cpp
Normal file
148
source/dcdt/se/se_dcdt_mesh.cpp
Normal file
@ -0,0 +1,148 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include "se_dcdt.h"
|
||||
|
||||
//============================== SeDcdtVertex ==================================
|
||||
|
||||
static void insert_if_not_there ( SrArray<int>& a, int id )
|
||||
{
|
||||
int i=0;
|
||||
for ( i=0; i<a.size(); i++ )
|
||||
if ( a[i]==id ) return;
|
||||
a.push() = id;
|
||||
}
|
||||
|
||||
static void insert_ids ( SrArray<int>& a, const SrArray<int>& ids )
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<ids.size(); i++ ) insert_if_not_there ( a, ids[i] );
|
||||
}
|
||||
|
||||
void SeDcdtVertex::get_references ( SrArray<int>& ids )
|
||||
{
|
||||
SeDcdtEdge *e;
|
||||
SeDcdtSymEdge *si, *s;
|
||||
|
||||
ids.size ( 0 );
|
||||
si = s = se();
|
||||
do { e = s->edg();
|
||||
if ( e->is_constrained() ) insert_ids ( ids, e->ids );
|
||||
s = s->rot();
|
||||
} while ( s!=si );
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& out, const SeDcdtVertex& v )
|
||||
{
|
||||
return out << v.p;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& inp, SeDcdtVertex& v )
|
||||
{
|
||||
return inp >> v.p;
|
||||
}
|
||||
|
||||
//=============================== SeDcdtEdge ==================================
|
||||
|
||||
bool SeDcdtEdge::has_id ( int id ) const
|
||||
{
|
||||
for ( int i=0; i<ids.size(); i++ )
|
||||
if ( ids[i]==id ) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SeDcdtEdge::has_other_id_than ( int id ) const
|
||||
{
|
||||
for ( int i=0; i<ids.size(); i++ )
|
||||
if ( ids[i]!=id ) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SeDcdtEdge::remove_id ( int id ) // remove all occurences
|
||||
{
|
||||
bool removed=false;
|
||||
int i=0;
|
||||
while ( i<ids.size() )
|
||||
{ if ( ids[i]==id )
|
||||
{ ids[i]=ids.pop(); removed=true; }
|
||||
else
|
||||
i++;
|
||||
}
|
||||
return removed;
|
||||
}
|
||||
|
||||
void SeDcdtEdge::add_constraints ( const SrArray<int>& cids )
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<cids.size(); i++ ) ids.push() = cids[i];
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& out, const SeDcdtEdge& e )
|
||||
{
|
||||
return out << e.ids;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& inp, SeDcdtEdge& e )
|
||||
{
|
||||
return inp >> e.ids;
|
||||
}
|
||||
|
||||
//============================== SeDcdtTriManager ==================================
|
||||
|
||||
void SeDcdtTriManager::get_vertex_coordinates ( const SeVertex* v, double& x, double & y )
|
||||
{
|
||||
x = (double) ((SeDcdtVertex*)v)->p.x;
|
||||
y = (double) ((SeDcdtVertex*)v)->p.y;
|
||||
}
|
||||
|
||||
void SeDcdtTriManager::set_vertex_coordinates ( SeVertex* v, double x, double y )
|
||||
{
|
||||
((SeDcdtVertex*)v)->p.x = (float) x;
|
||||
((SeDcdtVertex*)v)->p.y = (float) y;
|
||||
}
|
||||
|
||||
bool SeDcdtTriManager::is_constrained ( SeEdge* e )
|
||||
{
|
||||
return ((SeDcdtEdge*)e)->is_constrained();
|
||||
}
|
||||
|
||||
void SeDcdtTriManager::set_unconstrained ( SeEdge* e )
|
||||
{
|
||||
((SeDcdtEdge*)e)->set_unconstrained();
|
||||
}
|
||||
|
||||
void SeDcdtTriManager::get_constraints ( SeEdge* e, SrArray<int>& ids )
|
||||
{
|
||||
ids = ((SeDcdtEdge*)e)->ids;
|
||||
}
|
||||
|
||||
void SeDcdtTriManager::add_constraints ( SeEdge* e, const SrArray<int>& ids )
|
||||
{
|
||||
((SeDcdtEdge*)e)->add_constraints ( ids );
|
||||
}
|
||||
|
||||
//============================== SeDcdtTriManager ==================================
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SeDcdtInsPol& ob )
|
||||
{
|
||||
if (ob.open) o << "(open) ";
|
||||
return o;// << (const SrArray<SeDcdtVertex*>)ob;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SeDcdtInsPol& ob )
|
||||
{
|
||||
in.get_token();
|
||||
/*
|
||||
if ( in.last_token()[0]=='(' )
|
||||
{ ob.open = true;
|
||||
in.get_token();
|
||||
in.get_token();
|
||||
}
|
||||
else
|
||||
{ ob.open = false;
|
||||
in.unget_token();
|
||||
}
|
||||
*/
|
||||
return in;// >> (SrArray<SeDcdtVertex*>)ob;
|
||||
}
|
||||
|
||||
//=============================== End of File ===============================
|
||||
237
source/dcdt/se/se_mesh.cpp
Normal file
237
source/dcdt/se/se_mesh.cpp
Normal file
@ -0,0 +1,237 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
# include "se_mesh.h"
|
||||
|
||||
# define IFTYPE(t,v,e,f) t==TypeVertex? (v) : t==TypeEdge? (e) : (f)
|
||||
# define GETINFO(t,x) t==TypeVertex? (SeElement*)x->vertex : t==TypeEdge? (SeElement*)x->edge : (SeElement*)x->face
|
||||
|
||||
//================================= static functions =====================================
|
||||
|
||||
static void fatal_error ( char* msg )
|
||||
{
|
||||
sr_out.fatal_error ( msg );
|
||||
}
|
||||
|
||||
//============================= SeMeshBase Private Methods =====================================
|
||||
|
||||
void SeMeshBase::_defaults ()
|
||||
{
|
||||
_first = 0;
|
||||
_op_last_msg = OpNoErrors;
|
||||
_curmark = 0;
|
||||
_marking = false;
|
||||
_indexing = false;
|
||||
_vertices = _edges = _faces = 0;
|
||||
}
|
||||
|
||||
SeBase* SeMeshBase::_op_error ( OpMsg m )
|
||||
{
|
||||
_op_last_msg = (OpMsg)m;
|
||||
|
||||
if ( _output_op_errors )
|
||||
{ sr_out << "Error in SeMeshBase operation: "
|
||||
<< translate_op_msg(m) << srnl;
|
||||
}
|
||||
|
||||
return (SeBase*) 0;
|
||||
}
|
||||
|
||||
//============================ SeMeshBase general =====================================
|
||||
|
||||
semeshindex se_index_max_value = ((semeshindex)0)-1; // This is the unsigned greatest value;
|
||||
|
||||
SeMeshBase::SeMeshBase ( SrClassManagerBase* vtxman, SrClassManagerBase* edgman, SrClassManagerBase* facman )
|
||||
{
|
||||
_defaults ();
|
||||
_output_op_errors = 0;
|
||||
if ( !vtxman || !edgman || !facman )
|
||||
fatal_error ( "SeMeshBase::SeMeshBase(): null pointer received!" );
|
||||
_vtxman = vtxman;
|
||||
_edgman = edgman;
|
||||
_facman = facman;
|
||||
_op_check_parms = 0;
|
||||
}
|
||||
|
||||
SeMeshBase::~SeMeshBase ()
|
||||
{
|
||||
destroy ();
|
||||
_vtxman->unref();
|
||||
_edgman->unref();
|
||||
_facman->unref();
|
||||
}
|
||||
|
||||
/* from [mantyla] :
|
||||
v-e+f = h0-h1+h2 ( Betti numbers )
|
||||
h0 = shells
|
||||
h1 = 2 * genus ( every closed curve cut a part of the surface away )
|
||||
h2 = orientability = h0
|
||||
==> v-e+f = s-2g+s = 2s-2g = 2(s-g), if s==1, => v-e+f=2-2g */
|
||||
int SeMeshBase::euler () const
|
||||
{
|
||||
return _vertices - _edges + _faces;
|
||||
}
|
||||
|
||||
int SeMeshBase::genus () const
|
||||
{
|
||||
return (_vertices-_edges+_faces-2)/(-2);
|
||||
}
|
||||
|
||||
int SeMeshBase::elems ( ElemType type ) const
|
||||
{
|
||||
switch ( type )
|
||||
{ case TypeVertex : return _vertices;
|
||||
case TypeEdge : return _edges;
|
||||
case TypeFace : return _faces;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void SeMeshBase::invert_faces ()
|
||||
{
|
||||
SeBase *se;
|
||||
SeElement *edg, *edgi;
|
||||
int i, j, symedges = _edges*2;
|
||||
|
||||
if ( !_first ) return;
|
||||
|
||||
SrArray<SeBase*> nse(symedges,symedges);
|
||||
SrArray<SeBase*> nxt(symedges,symedges);
|
||||
SrArray<SeBase*> rot(symedges,symedges);
|
||||
SrArray<SeVertex*> vtx(symedges,symedges);
|
||||
|
||||
i=0;
|
||||
edg = edgi = _first->edg();
|
||||
do { se = edg->se();
|
||||
for ( j=0; j<2; j++ )
|
||||
{ if ( j==1 ) se = se->sym();
|
||||
vtx[i] = se->nxt()->vtx();
|
||||
nxt[i] = se->pri();
|
||||
rot[i] = se->nxt()->sym();
|
||||
nse[i] = se;
|
||||
i++;
|
||||
}
|
||||
edg = edg->nxt();
|
||||
} while ( edg!=edgi );
|
||||
|
||||
for ( i=0; i<symedges; i++ )
|
||||
{ //fg_out<<i<<" "<<(int)nse[i]<<fgnl;
|
||||
nse[i]->_next = nxt[i];
|
||||
nse[i]->_rotate = rot[i];
|
||||
nse[i]->_vertex = vtx[i];
|
||||
vtx[i]->_symedge = nse[i];
|
||||
}
|
||||
}
|
||||
|
||||
int SeMeshBase::vertices_in_face ( SeBase *e ) const
|
||||
{
|
||||
int i;
|
||||
SeBase *x=e->nxt();
|
||||
for ( i=1; x!=e; i++ ) x=x->nxt();
|
||||
return i;
|
||||
}
|
||||
|
||||
int SeMeshBase::vertex_degree ( SeBase *e ) const
|
||||
{
|
||||
int i=1;
|
||||
SeBase *x;
|
||||
for ( x=e->rot(); x!=e; i++ ) x=x->rot();
|
||||
return i;
|
||||
}
|
||||
|
||||
float SeMeshBase::mean_vertex_degree () const
|
||||
{
|
||||
if ( !_vertices ) return 0;
|
||||
|
||||
SeVertex* vi = _first->vtx();
|
||||
SeVertex* v = vi;
|
||||
float deg=0;
|
||||
do { deg += vertex_degree(v->se());
|
||||
v = v->nxt();
|
||||
} while ( v!=vi );
|
||||
|
||||
return deg / (float)_vertices;
|
||||
}
|
||||
|
||||
//============================ SeMeshBase marking =====================================
|
||||
|
||||
// replace _marking and _indexing by _mark_status !!
|
||||
|
||||
void SeMeshBase::_normalize_mark () // private method
|
||||
{
|
||||
SeElement *ei, *e;
|
||||
|
||||
_curmark=1;
|
||||
|
||||
if ( !_first ) return;
|
||||
|
||||
ei=e=_first->vtx(); do { e->_index=0; e=e->nxt(); } while ( e!=ei );
|
||||
ei=e=_first->edg(); do { e->_index=0; e=e->nxt(); } while ( e!=ei );
|
||||
ei=e=_first->fac(); do { e->_index=0; e=e->nxt(); } while ( e!=ei );
|
||||
}
|
||||
|
||||
void SeMeshBase::begin_marking ()
|
||||
{
|
||||
if ( _indexing || _marking )
|
||||
fatal_error("SeMeshBase::begin_marking() not allowed as marking or indexing is already in use!");
|
||||
|
||||
_marking = true;
|
||||
|
||||
if ( _curmark==se_index_max_value )
|
||||
{ _normalize_mark ();
|
||||
}
|
||||
else _curmark++;
|
||||
}
|
||||
|
||||
void SeMeshBase::end_marking ()
|
||||
{
|
||||
_marking = false;
|
||||
}
|
||||
|
||||
bool SeMeshBase::marked ( SeElement* e )
|
||||
{
|
||||
if ( !_marking ) fatal_error ( "SeMeshBase::marked(e): marking is not active!\n" );
|
||||
return e->_index==_curmark? true:false;
|
||||
}
|
||||
|
||||
void SeMeshBase::mark ( SeElement* e )
|
||||
{
|
||||
if ( !_marking ) fatal_error ( "SeMeshBase::mark(e): marking is not active!\n" );
|
||||
e->_index = _curmark;
|
||||
}
|
||||
|
||||
void SeMeshBase::unmark ( SeElement* e )
|
||||
{
|
||||
if ( !_marking ) fatal_error ( "SeMeshBase::unmark(e): marking is not active!\n");
|
||||
e->_index = _curmark-1;
|
||||
}
|
||||
|
||||
//============================ SeMeshBase indexing =====================================
|
||||
|
||||
void SeMeshBase::begin_indexing ()
|
||||
{
|
||||
if ( _marking || _indexing )
|
||||
fatal_error("SeMeshBase::begin_indexing() not allowed as marking or indexing is already in use!");
|
||||
_indexing = true;
|
||||
}
|
||||
|
||||
void SeMeshBase::end_indexing ()
|
||||
{
|
||||
_indexing = false;
|
||||
_normalize_mark ();
|
||||
}
|
||||
|
||||
semeshindex SeMeshBase::index ( SeElement* e )
|
||||
{
|
||||
if ( !_indexing ) fatal_error ("SeMeshBase::index(e): indexing is not active!");
|
||||
return e->_index;
|
||||
}
|
||||
|
||||
void SeMeshBase::index ( SeElement* e, semeshindex i )
|
||||
{
|
||||
if ( !_indexing ) fatal_error ("SeMeshBase::index(e,i): indexing is not active!");
|
||||
e->_index = i;
|
||||
}
|
||||
|
||||
//=== End of File ===================================================================
|
||||
419
source/dcdt/se/se_mesh.h
Normal file
419
source/dcdt/se/se_mesh.h
Normal file
@ -0,0 +1,419 @@
|
||||
|
||||
# ifndef SE_MESH_H
|
||||
# define SE_MESH_H
|
||||
|
||||
/** \file se_mesh.h
|
||||
* Symmetrical Edge Mesh Data Structure */
|
||||
|
||||
# include "sr_array.h"
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
# include "sr_class_manager.h"
|
||||
|
||||
# include "se.h"
|
||||
|
||||
// ====================================== SeMeshBase ======================================
|
||||
|
||||
// SeMeshBase documentation in the end of this file
|
||||
class SeMeshBase
|
||||
{ public :
|
||||
/*! Enumerator with the possible results of check_all(). */
|
||||
enum ChkMsg { ChkOk, //!< The check_all() method had success
|
||||
ChkNxtError, //!< Some face loop defined by the nxt() operators is wrong
|
||||
ChkRotError, //!< Some vertex loop defined by the rot() operators is wrong
|
||||
ChkVtxError, //!< Vertex referencing is wrong
|
||||
ChkEdgError, //!< Edge referencing is wrong
|
||||
ChkFacError //!< Face referencing is wrong
|
||||
};
|
||||
|
||||
/*! Enumerator with possible errors raised during the use of the topological operators. */
|
||||
enum OpMsg { OpNoErrors, //!< No Errors occured with operators
|
||||
OpMevParmsEqual, //!< Parms in mev are equal
|
||||
OpMevNotSameVtx, //!< Parms in mev not in the same vertex
|
||||
OpMefNotSameFace, //!< Parms in mef not in the same face
|
||||
OpKevLastEdge, //!< Cannot delete the last edge with kev
|
||||
OpKevManyEdges, //!< More then one edge in parm vertex in kev
|
||||
OpKevOneEdge, //!< Vertex of parm has only one edge in kev
|
||||
OpKefSameFace, //!< Parm in kef must be between two different faces
|
||||
OpMgUncompatibleFaces,//!< Parms are adjacent to faces with diferent number of vertices
|
||||
OpMgSameFace, //!< Parm in mg must be between two different faces
|
||||
OpFlipOneFace, //!< Parm in flip must be between two different faces
|
||||
OpEdgColOneFace, //!< Parm in EdgCol must be between two different faces
|
||||
OpVtxSplitNotSameVtx, //!< Parms in vtxsplit are not in the same vertex
|
||||
OpVtxSplitParmsEqual //!< Parms in VtxSplit are equal
|
||||
};
|
||||
|
||||
/*! Enumerator with the type of elements used in many methods arguments. */
|
||||
enum ElemType { TypeVertex,
|
||||
TypeEdge,
|
||||
TypeFace
|
||||
};
|
||||
|
||||
private : // Data ===================================================================================
|
||||
|
||||
SeBase* _first; // A pointer to the symedge of the mesh that is considered the first
|
||||
char _op_check_parms; // Flag to tell if must check operators parameters (default false)
|
||||
char _op_last_msg; // Keeps the last result of an operator (OpMsg type)
|
||||
char _output_op_errors; // Flag that outputs all errors ocurred during operators (default true)
|
||||
int _vertices, _edges, _faces; // Elem counters
|
||||
semeshindex _curmark; // Current values for marking
|
||||
char _marking, _indexing; // flags to indicate that marking or indexing is on
|
||||
SrClassManagerBase *_vtxman, *_edgman, *_facman; // keep used managers
|
||||
|
||||
private : // Private methods ==========================================================================
|
||||
void _defaults ();
|
||||
SeBase* _op_error ( OpMsg m );
|
||||
|
||||
public : // General mesh functions ===================================================================
|
||||
|
||||
/*! Inits an empty mesh. It requires to receive three manager classes
|
||||
dynamically allocated with operator new. These classes will manage data
|
||||
stored in vertices, edges and faces. If no associated data is required,
|
||||
the user can simply pass as argument "new SrClassManagerMeshElementManager", otherwise
|
||||
an allocated user-derived object must be created and passed to this
|
||||
constructor. SeMeshBase destructor will automatically delete them. */
|
||||
SeMeshBase ( SrClassManagerBase* vtxman, SrClassManagerBase* edgman, SrClassManagerBase* facman );
|
||||
|
||||
/*! Deletes all associated data: internal buffer, all mesh and its infos. */
|
||||
virtual ~SeMeshBase ();
|
||||
|
||||
/*! Returns the initial symedge of the mesh, can be 0. */
|
||||
SeBase* first () const { return _first; }
|
||||
|
||||
/*! Returns true if the mesh is empty, what happens when first() is null. */
|
||||
bool empty () const { return _first? false:true; }
|
||||
|
||||
/*! Returns v-e+f. */
|
||||
int euler () const;
|
||||
|
||||
/*! Returns (v-e+f-2)/(-2). */
|
||||
int genus () const;
|
||||
|
||||
/*! Returns the number of elements of the given type. */
|
||||
int elems ( ElemType type ) const;
|
||||
|
||||
/*! Returns the number of vertices in the mesh. */
|
||||
int vertices () const { return _vertices; }
|
||||
|
||||
/*! Returns the number of edges in the mesh. */
|
||||
int edges () const { return _edges; }
|
||||
|
||||
/*! Returns the number of faces in the mesh. */
|
||||
int faces () const { return _faces; }
|
||||
|
||||
/*! Invert orientation of all faces. */
|
||||
void invert_faces ();
|
||||
|
||||
/*! Returns the number of vertices in the face adjacent of the given symedge e. */
|
||||
int vertices_in_face ( SeBase* e ) const;
|
||||
|
||||
/*! Returns the number of edges incident to the vertex adjacent of the given symedge e,
|
||||
ie, the degree of the vertex. */
|
||||
int vertex_degree ( SeBase* e ) const;
|
||||
|
||||
/*! Returns the mean vertex degree of the whole mesh */
|
||||
float mean_vertex_degree () const;
|
||||
|
||||
/*! Returns true if the face incident to the given symedge e has exactly three edges. */
|
||||
bool is_triangle ( SeBase* e ) const { return e->nxt()->nxt()->nxt()==e? true:false; }
|
||||
|
||||
private : // Marking and Indexing =====================================================
|
||||
void _normalize_mark ();
|
||||
|
||||
public :
|
||||
|
||||
/*! Allows the user to safely mark elements with mark(), marked() and unmark()
|
||||
methods. Marking elements is necessary for many algorithms.
|
||||
SeMeshBase mantains a current marker value, so that each time begin_marking()
|
||||
is called, the marker value is incremented, guaranteing that old marked
|
||||
elements have a different mark value.
|
||||
When the max marking value is reached, a global normalization is done.
|
||||
Attention: Marking and indexing can not be used at the same time. Also,
|
||||
end_marking() must be called when marking operations are finished! */
|
||||
void begin_marking ();
|
||||
|
||||
/*! Must be called after a begin_marking() call, when the user finishes using the
|
||||
marking methods. Note: There is no problem if end_marking() is called without
|
||||
a previously call to begin_marking() */
|
||||
void end_marking ();
|
||||
|
||||
/*! See if an element is marked (begin_marking() should be called first) */
|
||||
bool marked ( SeElement* e );
|
||||
|
||||
/*! Unmark an element (begin_marking() should be called first) */
|
||||
void unmark ( SeElement* e );
|
||||
|
||||
/*! Mark an element (begin_marking() should be called first) */
|
||||
void mark ( SeElement* e );
|
||||
|
||||
/*! Indexing methods can be used only after a call to begin_indexing(), which permits
|
||||
the user to associate any index to any element. Note however that after a call to
|
||||
begin_indexing() the existant indices will have undefined values. This happens
|
||||
because indices and markers share the same variables, so that both can not be used
|
||||
at a same time. Method end_marking() must always be called after */
|
||||
void begin_indexing ();
|
||||
|
||||
/*! To finish indexing mode, end_indexing() must be called. The method will do a global
|
||||
normalization of the internal indices values, preparing them to be used for a later
|
||||
marking or indexing session. It can be called without a previous begin_marking()
|
||||
call, but note that it will always peform the global normalization. */
|
||||
void end_indexing ();
|
||||
|
||||
/*! Retrieves the index of an element (begin_indexing() should be called first) */
|
||||
semeshindex index ( SeElement* e );
|
||||
|
||||
/*! Sets the index of an element (begin_indexing() should be called first) */
|
||||
void index ( SeEdge* e, semeshindex i );
|
||||
|
||||
public : // Operators Control ===========================================================================
|
||||
|
||||
/*! Get safe mode status. When it is true, error msgs are generated, but
|
||||
operations become a little slower because some minor checkings are
|
||||
performed. The default mode is true only for the
|
||||
debug version of the library (compiled with _DEBUG macro defined).*/
|
||||
bool safe_mode () { return _op_check_parms? true:false; }
|
||||
|
||||
/*! Change the safe mode status. The default mode is false. */
|
||||
void safe_mode ( bool b ) { _op_check_parms=(char)b; }
|
||||
|
||||
/*! Say if error messages should be printed or not (using sr_out), default is false. */
|
||||
void output_errors ( bool b ) { _output_op_errors=(char)b; }
|
||||
|
||||
/*! Whenever an operator returns null, an error message is generated and
|
||||
can be retrieved here. Each time an operator is called, a new message
|
||||
is generated. And whenever the operator finishes succesfully OpNoErrors
|
||||
is generated. */
|
||||
OpMsg last_op_msg () { return (OpMsg)_op_last_msg; }
|
||||
|
||||
/*! Get a string description of the given message. */
|
||||
static char *translate_op_msg ( OpMsg m );
|
||||
|
||||
/*! Makes a global consistency check, for debug purposes, should always return ChkOk. */
|
||||
ChkMsg check_all () const;
|
||||
|
||||
private :
|
||||
void _new_vtx(SeBase* s); void _new_edg(SeBase* s); void _new_fac(SeBase* s);
|
||||
void _del_vtx(SeElement* e); void _del_edg(SeElement* e); void _del_fac(SeElement* e);
|
||||
|
||||
public : // Operators ==================================================================================
|
||||
|
||||
/*! Destroy all the mesh and associated data. */
|
||||
void destroy ();
|
||||
|
||||
/*! Initialize the mesh. See also SeMeshBase documentation. */
|
||||
SeBase* init ();
|
||||
|
||||
/*! Make edge and vertex operator. See also SeMeshBase documentation. */
|
||||
SeBase* mev ( SeBase* s );
|
||||
|
||||
/*! Make edge and vertex operator, splitting a vertex loop. See also SeMeshBase documentation. */
|
||||
SeBase* mev ( SeBase* s1, SeBase* s2 );
|
||||
|
||||
/*! Make edge and face operator. See also SeMeshBase documentation. */
|
||||
SeBase* mef ( SeBase* s1, SeBase* s2 );
|
||||
|
||||
/*! Kill edge and vertex operator. See also SeMeshBase documentation. */
|
||||
SeBase* kev ( SeBase* x );
|
||||
|
||||
/*! Kill edge and vertex operator that can join two vertex loops. See also SeMeshBase documentation. */
|
||||
SeBase* kev ( SeBase* x, SeBase* *s );
|
||||
|
||||
/*! Kill edge and face operator. See also SeMeshBase documentation. */
|
||||
SeBase* kef ( SeBase* x, SeBase* *s );
|
||||
|
||||
/*! Make genus by joining two face contours and deleting two faces,
|
||||
vertices and edges of s2 face. s1 and s2 must be symmetrically
|
||||
placed, each one on its contour to join. (not fully tested) */
|
||||
SeBase* mg ( SeBase* s1, SeBase* s2 );
|
||||
|
||||
/*! Flip the edge of a face. See also SeMeshBase documentation. */
|
||||
SeBase* flip ( SeBase* x );
|
||||
|
||||
/*! Edge collide operator. See also SeMeshBase documentation. */
|
||||
SeBase* ecol ( SeBase* x, SeBase* *s );
|
||||
|
||||
/*! Vertex split operator. See also SeMeshBase documentation. */
|
||||
SeBase* vsplit ( SeBase* s1, SeBase* s2 );
|
||||
|
||||
/*! Add vertex operator. See also SeMeshBase documentation. */
|
||||
SeBase* addv ( SeBase* x );
|
||||
|
||||
/*! Delete vertex operator. See also SeMeshBase documentation. */
|
||||
SeBase* delv ( SeBase* y );
|
||||
|
||||
private : // IO ========================================================================================
|
||||
void _elemsave ( SrOutput& out, ElemType type, SeElement* first );
|
||||
void _elemload ( SrInput& inp, SrArray<SeElement*>& e, const SrArray<SeBase*>& s, SrClassManagerBase* man );
|
||||
|
||||
public :
|
||||
|
||||
/*! Writes the mesh to the given output. save() will invoke the
|
||||
virtual methods output() of the associated element managers.
|
||||
Note: indexing is used during save(). */
|
||||
bool save ( SrOutput& out );
|
||||
|
||||
/*! Reads the mesh from the given input. load() will invoke the
|
||||
virtual methods input() of the associated element managers.
|
||||
Note: indexing is used during load(). */
|
||||
bool load ( SrInput& inp );
|
||||
};
|
||||
|
||||
/*! This is the template version of the SeMeshBase class, that redefines some methods
|
||||
of SeBase only for the purpose of including type casts to user defined classes.
|
||||
All methods are implemented inline just calling the corresponding method of
|
||||
the base class but correctly applying type casts to convert default types
|
||||
to user types. It must be used together with SeTpl. */
|
||||
template <class V, class E, class F> //, class VM, class EM, class FM>
|
||||
class SeMesh : public SeMeshBase
|
||||
{ public :
|
||||
typedef Se<V,E,F> S;
|
||||
typedef SrClassManager<V> VM;
|
||||
typedef SrClassManager<E> EM;
|
||||
typedef SrClassManager<F> FM;
|
||||
|
||||
SeMesh () : SeMeshBase ( new VM, new EM, new FM ) {}
|
||||
|
||||
S* first () const { return (S*)SeMeshBase::first(); }
|
||||
|
||||
S* init () { return (S*)SeMeshBase::init(); }
|
||||
S* mev ( S* s ) { return (S*)SeMeshBase::mev(s); }
|
||||
S* mev ( S* s1, S* s2 ) { return (S*)SeMeshBase::mev(s1,s2); }
|
||||
S* mef ( S* s1, S* s2 ) { return (S*)SeMeshBase::mef(s1,s2); }
|
||||
S* kev ( S* x ) { return (S*)SeMeshBase::kev(x); }
|
||||
S* kev ( S* x, S* *s ) { return (S*)SeMeshBase::kev(x,(SeBase**)s); }
|
||||
S* kef ( S* x, S* *s ) { return (S*)SeMeshBase::kef(x,(SeBase**)s); }
|
||||
S* mg ( S* s1, S* s2 ) { return (S*)SeMeshBase::mg(s1,s2); }
|
||||
S* flip ( S* x ) { return (S*)SeMeshBase::flip(x); }
|
||||
S* ecol ( S* x, S* *s ) { return (S*)SeMeshBase::ecol(x,(SeBase**)s); }
|
||||
S* vsplit ( S* s1, S* s2 ) { return (S*)SeMeshBase::vsplit(s1,s2); }
|
||||
S* addv ( S* x ) { return (S*)SeMeshBase::addv(x); }
|
||||
S* delv ( S* y ) { return (S*)SeMeshBase::delv(y); }
|
||||
};
|
||||
|
||||
/*! \class SeMeshBase se_mesh.h
|
||||
\brief manages topology and attached information of a symedge mesh.
|
||||
|
||||
SeMeshBase uses SeBase as argument for many methods, specially for the
|
||||
topological operators. Each SeBase instance has only one vertex, edge
|
||||
and face adjacent (or associated) and so it can be used to reference any
|
||||
of these elements. SeMeshBase destructor calls destroy(), that will call also
|
||||
each free() method of registered SeElementManagers in the mesh. To attach info
|
||||
to a mesh, the user must derive SrClassManagerBase, suply the required virtual
|
||||
methods, and push it in SeMeshBase with push_info() method.
|
||||
|
||||
Operators error handling is performed when the checking mode is true (the
|
||||
default), all operators may then return a 0 (null) pointer when their
|
||||
parameters are illegal. In such cases, a call to last_op_msg() will return
|
||||
the error occured. Next is a schema explaining each operator: \code
|
||||
|
||||
*========================================================================
|
||||
* Operator init()
|
||||
*
|
||||
* x=init() v1 o o v2 -> v1 o------o v2
|
||||
* destroy() <-----
|
||||
* destroy all the previous structure. x
|
||||
*
|
||||
|
||||
*========================================================================
|
||||
* Operator mev(s)
|
||||
*
|
||||
* s x
|
||||
* <----- <-----
|
||||
* x=mev(s) o------o o v -> o------o------o v
|
||||
* s=kev(x) / | / |
|
||||
* / | / |
|
||||
* To kev(x), only o o o o
|
||||
* one edge in v;
|
||||
*
|
||||
|
||||
*========================================================================
|
||||
* Operator mev(s1,s2)
|
||||
* o o
|
||||
* | \
|
||||
* | s1 \
|
||||
* | ----> \
|
||||
* x=mev(s1,s2) o------o---------o -> o------o------o--o
|
||||
* s1=kev(x,&s2) <---- | o | <--- v
|
||||
* s2 | v | x
|
||||
* To mev, s1 & s2 on o o
|
||||
* the same vertex
|
||||
* and s1!=s2
|
||||
*
|
||||
* To kev(x), more than one edge of x must be incident to v
|
||||
*
|
||||
|
||||
*=========================================================================
|
||||
* Operator mef(s1,s2)
|
||||
*
|
||||
* x=mef(s1,s2) o-------o-------o o-------o-------o
|
||||
* s1=kef(x,&s2) | <---- | | | |
|
||||
* | s1 | | | |
|
||||
* | | | || |
|
||||
* s1 & s2 on | | -> | e||x |
|
||||
* same face | | | |V |
|
||||
* | s2 | | | f |
|
||||
* in kef(x), x->sym | ----> | | | |
|
||||
* must be in a diff o-------o-------o o-------o-------o
|
||||
* face than x
|
||||
*
|
||||
|
||||
*========================================================================
|
||||
* Operator flip(x)
|
||||
*
|
||||
* y=flip(x) o---o---o o---o---o
|
||||
* / | | / |
|
||||
* / | | / |
|
||||
* x & x->sym must / || | / |
|
||||
* be in diff faces o ||x o -> o---------------o
|
||||
* and share the | |V / | <------ /
|
||||
* same edge | | / | y /
|
||||
* | | / | /
|
||||
* o---o---o o---o---o
|
||||
*
|
||||
* This operator acts as an "edge rotation"; that in triangulations is the
|
||||
* same as the well known flip (or swap) operator.
|
||||
*
|
||||
|
||||
*========================================================================
|
||||
* Operator ecol(x,&s2)
|
||||
*
|
||||
* s1=ecol(x,&s2) o o
|
||||
* x=vsplit(s1,s2) / | s1|
|
||||
* / | ^|
|
||||
* \ / x | / \ || /
|
||||
* x and x->sym must \ / --> |/ \ |/
|
||||
* be in diff faces o---------o v -> o v
|
||||
* and share the / \ |\ / |\
|
||||
* same edge / \ | \ / | \
|
||||
* / \ | / / |
|
||||
* / \ | / /s2 |
|
||||
* o V o
|
||||
*
|
||||
* In the process, 3 edges, 2 faces and 1 vertex are created/destroyed.
|
||||
* This operator acts as an "edge collapse", and has
|
||||
* the "vertex split" operator as inverse.
|
||||
* The three destroyed edges are incident to x->vtx(), and not v.
|
||||
*
|
||||
|
||||
*========================================================================
|
||||
* Operator addv()
|
||||
*
|
||||
* y=addv(x) ----------- -----------
|
||||
* x=delv(y) | <-- | |\ /|
|
||||
* | x | | \ y^/ |
|
||||
* | | | \ // |
|
||||
* | | | \ / |
|
||||
* | o | -> | o |
|
||||
* | | | / \ |
|
||||
* | | | / \ |
|
||||
* | | | / \ |
|
||||
* | | |/ \|
|
||||
* ----------- -----------
|
||||
*
|
||||
*
|
||||
\endcode */
|
||||
|
||||
//============================ End of File ===============================
|
||||
|
||||
# endif // SE_MESH_H
|
||||
|
||||
535
source/dcdt/se/se_mesh_import.cpp
Normal file
535
source/dcdt/se/se_mesh_import.cpp
Normal file
@ -0,0 +1,535 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include "sr_tree.h"
|
||||
# include "se_mesh_import.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // basic messages
|
||||
//# define SR_USE_TRACE2 // more messages
|
||||
//# define SR_USE_TRACE4 // active contour
|
||||
//# define SR_USE_TRACE5 // tree
|
||||
# include "sr_trace.h"
|
||||
|
||||
//======================= static data =============================================
|
||||
|
||||
// Question: Could I substitute EdgeTree by an EdgeHashTable ...?
|
||||
|
||||
class Edge : public SrTreeNode
|
||||
{ public :
|
||||
int a, b; // Edge vertices, not oriented
|
||||
int v; // The other vertex of the adjacent face
|
||||
int f; // The index of the edge's face
|
||||
Edge *sym; // The other edge, belonging to the other face sharing this edge
|
||||
Edge *e2, *e3; // The other two edges of this face
|
||||
public :
|
||||
Edge ( int i1=0, int i2=0, int i3=0 ) : f(0), sym(0), e2(0), e3(0) { a=i1; b=i2; v=i3; }
|
||||
void set_fac_elems ( Edge *b, Edge *c, int fac ) { e2=b; e3=c; f=fac; }
|
||||
void swap() { int tmp=a; a=b; b=tmp; }
|
||||
|
||||
friend SrOutput& operator<< ( SrOutput& o, const Edge& e )
|
||||
{ return o<<'['<<e.a<<' '<<e.b<<' '<<e.v<<"]"; }
|
||||
|
||||
friend SrInput& operator>> ( SrInput& i, Edge& e ) { return i; }
|
||||
|
||||
friend int sr_compare ( const Edge* e1, const Edge* e2 );
|
||||
};
|
||||
|
||||
int sr_compare ( const Edge *e1, const Edge *e2 )
|
||||
{
|
||||
if ( e1->a<e2->a ) return -1;
|
||||
if ( e1->a>e2->a ) return 1;
|
||||
if ( e1->b<e2->b ) return -1;
|
||||
if ( e1->b>e2->b ) return 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
# ifdef SE_USR_TRACE4
|
||||
struct PFace { SeBase *e; SeMeshBase *m; } PFaceInst;
|
||||
FgOutput &operator << ( FgOutput &o, PFace &f )
|
||||
{
|
||||
o << '[';
|
||||
SeBase *e=f.e;
|
||||
while ( true )
|
||||
{ o<<(int)f.m->index(e->vtx());
|
||||
e = e->nxt();
|
||||
if ( e!=f.e ) o<<' '; else break;
|
||||
}
|
||||
return o<<']';
|
||||
}
|
||||
# define SR_TRACE_CONTOUR(x,s) { PFaceInst.e=x; PFaceInst.m=s; SR_TRACE4 ( "Active Contour = "<< PFaceInst ); }
|
||||
# else
|
||||
# define SR_TRACE_CONTOUR(x,s)
|
||||
# endif
|
||||
*/
|
||||
|
||||
# define SR_TRACE_CONTOUR(x,s)
|
||||
|
||||
//=========================== EdgeTree ========================================
|
||||
|
||||
class EdgeTree : public SrTree<Edge>
|
||||
{ public :
|
||||
// EdgeTree() {}
|
||||
public :
|
||||
Edge *insert_edge ( int a, int b, int v );
|
||||
bool insert_face ( int a, int b, int c, int f );
|
||||
Edge* search_edge ( int a, int b );
|
||||
void remove_face ( Edge *e );
|
||||
};
|
||||
|
||||
Edge *EdgeTree::insert_edge ( int a, int b, int v )
|
||||
{
|
||||
Edge *e = new Edge ( a, b, v );
|
||||
if ( !insert(e) )
|
||||
{ Edge *x = cur();
|
||||
e->swap();
|
||||
if ( !insert(e) )
|
||||
{ SR_TRACE2 ( "Could not insert edge ("<<a<<','<<b<<")!!" );
|
||||
delete e;
|
||||
return 0;
|
||||
}
|
||||
x->sym = e;
|
||||
e->sym = x;
|
||||
}
|
||||
else // search if the sym edge is there and set sym pointers
|
||||
{ // (to try: sort (a,b) to avoid checking if the sym exists...)
|
||||
Edge eba ( b, a, v );
|
||||
Edge *esym = search ( &eba );
|
||||
if ( esym )
|
||||
{ esym->sym = e;
|
||||
e->sym = esym;
|
||||
}
|
||||
}
|
||||
return e;
|
||||
}
|
||||
|
||||
bool EdgeTree::insert_face ( int a, int b, int c, int f )
|
||||
{
|
||||
Edge *e1 = insert_edge ( a, b, c );
|
||||
Edge *e2 = insert_edge ( b, c, a );
|
||||
Edge *e3 = insert_edge ( c, a, b );
|
||||
|
||||
if (e1) e1->set_fac_elems ( e2, e3, f );
|
||||
if (e2) e2->set_fac_elems ( e1, e3, f );
|
||||
if (e3) e3->set_fac_elems ( e1, e2, f );
|
||||
|
||||
return e1&&e2&&e3? true:false;
|
||||
}
|
||||
|
||||
Edge* EdgeTree::search_edge ( int a, int b )
|
||||
{
|
||||
Edge key(a,b,0);
|
||||
|
||||
Edge *e = search(&key);
|
||||
if ( !e )
|
||||
{ key.swap();
|
||||
e=search(&key);
|
||||
}
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
void EdgeTree::remove_face ( Edge *e )
|
||||
{
|
||||
if (!e) return;
|
||||
|
||||
// e2 and/or e3 can be null in case non-manifold edges were found during the
|
||||
// construction of the edge tree with start()
|
||||
Edge *e2 = e->e2;
|
||||
Edge *e3 = e->e3;
|
||||
|
||||
extract ( e );
|
||||
if ( e->sym ) e->sym->sym=0;
|
||||
|
||||
if (e2)
|
||||
{ if ( e2->sym ) e2->sym->sym=0;
|
||||
remove ( e2 );
|
||||
}
|
||||
|
||||
if (e3)
|
||||
{ if ( e3->sym ) e3->sym->sym=0;
|
||||
remove ( e3 );
|
||||
}
|
||||
}
|
||||
|
||||
//======================= internal data ==================================
|
||||
|
||||
static SeBase* init ( SeMeshImport *self, SeMeshBase* m, int a, int b )
|
||||
{
|
||||
SeBase *s = m->init ();
|
||||
m->index(s->vtx(),(semeshindex)b);
|
||||
m->index(s->nxt()->vtx(),(semeshindex)a);
|
||||
self->attach_vtx_info ( s->vtx(), b );
|
||||
self->attach_vtx_info ( s->nxt()->vtx(), a );
|
||||
self->attach_edg_info ( s->edg(), b, a );
|
||||
return s;
|
||||
}
|
||||
|
||||
static SeBase* mev ( SeMeshImport *self, SeMeshBase* m, SeBase* s, int ev, int v )
|
||||
{
|
||||
s = m->mev ( s );
|
||||
self->attach_edg_info ( s->edg(), ev, v );
|
||||
self->attach_vtx_info ( s->vtx(), v );
|
||||
m->index ( s->vtx(), (semeshindex)v );
|
||||
return s;
|
||||
}
|
||||
|
||||
static SeBase* mef ( SeMeshImport *self, SeMeshBase* m, SeBase* s1, SeBase* s2, int a, int b, int f )
|
||||
{
|
||||
s2 = m->mef ( s1, s2 );
|
||||
self->attach_edg_info ( s2->edg(), a, b );
|
||||
self->attach_fac_info ( s2->fac(), f );
|
||||
return s2;
|
||||
}
|
||||
|
||||
class SeMeshImport::Data
|
||||
{ public :
|
||||
SeMeshImport *self;
|
||||
EdgeTree tree;
|
||||
SrArray<SeBase*> active_contours; // stack of contours
|
||||
SrArray<SeBase*> contours_to_solve; // keep contours that will result in genus or holes
|
||||
SrArray<SeBase*> vtxsymedges; // keep pointer for vertices already inserted
|
||||
public :
|
||||
Data ( SeMeshImport *x );
|
||||
SeMeshBase *init_shell ();
|
||||
int start ( const int *triangles, int numtris, int numvtx );
|
||||
void solve_lasting_contours ( SeMeshBase *m );
|
||||
void glue_face_on_one_edge ( SeMeshBase *m, SeBase *&s, Edge *e );
|
||||
void glue_face_on_two_edges ( SeMeshBase *m, SeBase *&s, Edge *e );
|
||||
void glue_face_on_three_edges ( SeMeshBase *m, SeBase *s, Edge *e );
|
||||
};
|
||||
|
||||
SeMeshImport::Data::Data ( SeMeshImport *x )
|
||||
{
|
||||
vtxsymedges.size(0);
|
||||
vtxsymedges.capacity(256);
|
||||
active_contours.size(0);
|
||||
active_contours.capacity(32);
|
||||
self = x;
|
||||
}
|
||||
|
||||
SeMeshBase *SeMeshImport::Data::init_shell ()
|
||||
{
|
||||
SR_TRACE1 ( "Getting new shell..." );
|
||||
SeMeshBase *m = self->get_new_shell ();
|
||||
if ( !m ) return 0;
|
||||
|
||||
SR_TRACE1 ( "Getting seed face from the root of the tree..." );
|
||||
|
||||
Edge *e = tree.root();
|
||||
int a=e->a; int b=e->b; int c=e->v; // the order of the vertices is random
|
||||
int f=e->f;
|
||||
tree.remove_face ( e );
|
||||
delete e;
|
||||
|
||||
SR_TRACE1 ( "Creating seed face = ["<<a<<" "<<b<<" "<<c<<"]" );
|
||||
|
||||
m->begin_indexing ();
|
||||
|
||||
SeBase *s;
|
||||
|
||||
s = init ( self, m, a, b );
|
||||
s = mev ( self, m, s, b, c );
|
||||
mef ( self, m, s, s->nxt()->nxt(), a, c, f );
|
||||
|
||||
vtxsymedges[c] = s;
|
||||
vtxsymedges[b] = s->nxt();
|
||||
vtxsymedges[a] = s->nxt()->nxt();
|
||||
|
||||
active_contours.push() = s;
|
||||
|
||||
SR_TRACE2 ("seed face info: "<<(int)s->sym()->fac()<<", active face info: "<<(int)s->fac() );
|
||||
|
||||
return m;
|
||||
}
|
||||
|
||||
int SeMeshImport::Data::start ( const int *triangles, int numtris, int numvtx )
|
||||
{
|
||||
int i;
|
||||
|
||||
active_contours.size(0);
|
||||
|
||||
SR_TRACE1 ( "Initializing vertices table with size "<<numvtx<<"..." );
|
||||
vtxsymedges.size(numvtx);
|
||||
for ( i=0; i<vtxsymedges.size(); i++ ) vtxsymedges[i]=0;
|
||||
|
||||
SR_TRACE1 ( "Creating edge tree..." );
|
||||
|
||||
const int *tri=triangles;
|
||||
int faces_with_edges_not_inserted=0;
|
||||
for ( i=0; i<numtris; i++ )
|
||||
{ if ( !tree.insert_face(tri[0],tri[1],tri[2],i) )
|
||||
faces_with_edges_not_inserted++;
|
||||
tri += 3;
|
||||
}
|
||||
|
||||
return faces_with_edges_not_inserted;
|
||||
}
|
||||
|
||||
static SeBase* coincident_contour ( SeBase *s1, SeBase *s2 )
|
||||
{
|
||||
SeBase *x = s2;
|
||||
while ( s1->vtx() != x->nxt()->vtx() )
|
||||
{ x = x->nxt();
|
||||
if ( x==s2 ) return 0; // not coincident
|
||||
}
|
||||
|
||||
// a common starting vertex was found, so that the contours will be coincident
|
||||
// or we'll have non-manifold vertices. Let's verify :
|
||||
s2 = x;
|
||||
do { if ( s1->vtx() != s2->nxt()->vtx() )
|
||||
{ printf("Non-manifold vertex detected!\n"); return 0; }
|
||||
s1 = s1->nxt();
|
||||
s2 = s2->pri();
|
||||
} while ( s2!=x );
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
// should finished the contours by detecting loops to join or holes info to attach
|
||||
void SeMeshImport::Data::solve_lasting_contours ( SeMeshBase *m )
|
||||
{
|
||||
int i;
|
||||
bool genus_increased;
|
||||
SeBase *s, *c;
|
||||
|
||||
while ( !contours_to_solve.empty() )
|
||||
{ genus_increased = false;
|
||||
s = contours_to_solve.pop();
|
||||
SR_TRACE_CONTOUR(s,m);
|
||||
for ( i=0; i<contours_to_solve.size(); i++ )
|
||||
{ c = coincident_contour(s,contours_to_solve[i]);
|
||||
if ( c )
|
||||
{ SR_TRACE1 ( "Making one genus..." );
|
||||
SR_TRACE_CONTOUR(c,m);
|
||||
contours_to_solve.remove(i);//[i]=contours_to_solve.pop();
|
||||
genus_increased = true;
|
||||
m->mg ( s, c );
|
||||
break;
|
||||
}
|
||||
}
|
||||
if ( !genus_increased )
|
||||
{ SR_TRACE1 ( "Closing last contour as a hole..." );
|
||||
self->attach_hole_info ( s->fac() );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SeMeshImport::Data::glue_face_on_one_edge ( SeMeshBase *m, SeBase *&s, Edge *e )
|
||||
{
|
||||
tree.remove_face ( e );
|
||||
|
||||
SeBase *snxt = s->nxt();
|
||||
|
||||
s = mev ( self, m, s, e->a, e->v );
|
||||
s = mef ( self, m, snxt, s, e->b, e->v, e->f );
|
||||
|
||||
vtxsymedges[e->a] = s->nxt()->sym();
|
||||
vtxsymedges[e->b] = snxt;
|
||||
vtxsymedges[e->v] = snxt->pri();
|
||||
s = snxt;
|
||||
|
||||
delete e;
|
||||
}
|
||||
|
||||
void SeMeshImport::Data::glue_face_on_two_edges ( SeMeshBase* m, SeBase*& s, Edge* e )
|
||||
{
|
||||
tree.remove_face ( e );
|
||||
SeElement *ve = vtxsymedges[e->v]->vtx();
|
||||
SeBase *s1, *s2;
|
||||
|
||||
if ( s->pri()->vtx()==ve )
|
||||
{ s1=s->nxt(); s2=s->pri(); }
|
||||
else
|
||||
{ s1=s->nxt()->nxt(); s2=s; }
|
||||
|
||||
mef ( self, m, s1, s2, m->index(s1->vtx()), m->index(s2->vtx()), e->f );
|
||||
s = s1->pri();
|
||||
|
||||
vtxsymedges[(int)m->index(s->vtx())] = s;
|
||||
vtxsymedges[(int)m->index(s1->vtx())] = s1;
|
||||
delete e;
|
||||
}
|
||||
|
||||
void SeMeshImport::Data::glue_face_on_three_edges ( SeMeshBase* /*m*/, SeBase* s, Edge* e )
|
||||
{
|
||||
tree.remove_face ( e );
|
||||
self->attach_fac_info ( s->fac(), e->f );
|
||||
delete e;
|
||||
}
|
||||
|
||||
//======================== SeMeshImport ========================================
|
||||
|
||||
SeMeshImport::SeMeshImport ()
|
||||
{
|
||||
_data = new Data ( this );
|
||||
}
|
||||
|
||||
SeMeshImport::~SeMeshImport ()
|
||||
{
|
||||
for ( int i=0; i<shells.size(); i++ ) delete shells[i];
|
||||
delete _data;
|
||||
}
|
||||
|
||||
void SeMeshImport::compress_buffers ()
|
||||
{
|
||||
shells.compress();
|
||||
_data->vtxsymedges.capacity(0);
|
||||
_data->active_contours.capacity(0);
|
||||
}
|
||||
|
||||
SeMeshImport::Msg SeMeshImport::start ( const int *triangles, int numtris, int numvtxs )
|
||||
{
|
||||
SR_TRACE1 ( "Start :" );
|
||||
|
||||
int not_inserted = _data->start ( triangles, numtris, numvtxs );
|
||||
|
||||
shells.size(0);
|
||||
SeMeshBase *m = _data->init_shell (); // will push something in active_contours
|
||||
if ( !m ) return MsgNullShell;
|
||||
shells.push()=m;
|
||||
|
||||
SR_TRACE_CONTOUR(_data->active_contours.top(),m);
|
||||
SR_TRACE4 ( "Face info of active contour = " << (int)(_data->active_contours.top()->fac()) );
|
||||
SR_TRACE5 ( "Tree = "<<_data->tree );
|
||||
SR_TRACE1 ( "Start finished !" );
|
||||
SR_TRACE1 ( "Faces Lost: "<<not_inserted );
|
||||
|
||||
return not_inserted>0? MsgNonManifoldFacesLost : MsgOk;
|
||||
}
|
||||
|
||||
static SeBase* in_same_face ( SeBase *e, SeElement *v )
|
||||
{
|
||||
SeBase *x = e;
|
||||
do { if ( x->vtx()==v ) return x;
|
||||
x = x->nxt();
|
||||
} while ( x!=e );
|
||||
return 0;
|
||||
}
|
||||
|
||||
SeMeshImport::Msg SeMeshImport::next_step ()
|
||||
{
|
||||
int a, b;
|
||||
Edge *e;
|
||||
SeBase *ini, *s, *sv;
|
||||
|
||||
int edges_found = 0;
|
||||
|
||||
SR_TRACE2 ( " " );
|
||||
SR_TRACE2 ( "Next step :" );
|
||||
|
||||
EdgeTree &tree = _data->tree;
|
||||
SrArray<SeBase*> &active_contours = _data->active_contours;
|
||||
SrArray<SeBase*> &contours_to_solve = _data->contours_to_solve;
|
||||
SrArray<SeBase*> &vtxsymedges = _data->vtxsymedges;
|
||||
|
||||
SR_TRACE2 ( "Active contours : "<<active_contours.size() );
|
||||
|
||||
if ( tree.empty() )
|
||||
{ SR_TRACE1 ( "Empty tree: no more faces to glue." );
|
||||
if ( !active_contours.empty() )
|
||||
{ while ( !active_contours.empty() ) contours_to_solve.push()=active_contours.pop();
|
||||
SR_TRACE1 ( "Solving "<<contours_to_solve.size()<<" open contour(s)..." );
|
||||
_data->solve_lasting_contours ( shells.top() );
|
||||
}
|
||||
for ( int i=0; i<shells.size(); i++ ) shells[i]->end_indexing();
|
||||
SR_TRACE1 ( "Finished : "<<shells.size()<<" shell(s), last shell V-E+F="<<shells.top()->euler() );
|
||||
return MsgFinished;
|
||||
}
|
||||
else if ( active_contours.empty() )
|
||||
{ SR_TRACE1 ( "Starting new shell "<<shells.size()<<"... " );
|
||||
shells.push() = _data->init_shell();
|
||||
return MsgOk; // return now to cope with degenerated data (a shell with a single triangle).
|
||||
}
|
||||
|
||||
s = ini = active_contours.top();
|
||||
|
||||
while ( true )
|
||||
{
|
||||
a = (int) shells.top()->index(s->vtx());
|
||||
b = (int) shells.top()->index(s->nxt()->vtx());
|
||||
SR_TRACE2 ( "Searching edge "<<a<<" "<<b<<"... " );
|
||||
e = tree.search_edge ( a, b ); // will search {a,b} or {b,a}
|
||||
|
||||
if ( e )
|
||||
{ edges_found++;
|
||||
|
||||
sv = 0;
|
||||
if ( vtxsymedges[e->v] ) // the other vertex is already in the mesh
|
||||
sv = in_same_face ( s, vtxsymedges[e->v]->vtx() );
|
||||
|
||||
if ( !vtxsymedges[e->v] || !sv ) // if the other vertex is free or is in another contour
|
||||
{ SR_TRACE1 ( "Gluing face "<<a<<" "<<b<<" "<<e->v<<" on one edge... " );
|
||||
_data->glue_face_on_one_edge ( shells.top(), s, e );
|
||||
active_contours.top() = s;
|
||||
SR_TRACE_CONTOUR(s,shells.top());
|
||||
SR_TRACE5 ( "Tree = "<<tree );
|
||||
return MsgOk;
|
||||
}
|
||||
else if ( s->nxt()->nxt()->nxt()==s ) // trying to close the active contour that is a triangle
|
||||
{ SR_TRACE1 ( "Gluing last face "<<a<<" "<<b<<" "<<e->v<<" of current active contour..." );
|
||||
_data->glue_face_on_three_edges ( shells.top(), s, e );
|
||||
active_contours.pop();
|
||||
SR_TRACE1 ( "Active contour popped... " );
|
||||
SR_TRACE5 ( "Tree = "<<tree );
|
||||
return MsgOk;
|
||||
}
|
||||
else if ( s->nxt()->nxt()->vtx()==vtxsymedges[e->v]->vtx() ||
|
||||
s->pri()->vtx()==vtxsymedges[e->v]->vtx() )
|
||||
{ SR_TRACE1 ( "Gluing face "<<a<<" "<<b<<" "<<e->v<<" on two edges... " );
|
||||
_data->glue_face_on_two_edges ( shells.top(), s, e );
|
||||
active_contours.top() = s;
|
||||
SR_TRACE_CONTOUR(s,shells.top());
|
||||
SR_TRACE5 ( "Tree = "<<tree );
|
||||
return MsgOk;
|
||||
}
|
||||
else if ( sv )
|
||||
{ SR_TRACE1 ( "Connecting one edge, and pushing one active contour... " );
|
||||
shells.top()->mef ( s, sv );
|
||||
attach_edg_info ( s->edg(), shells.top()->index(s->vtx()), shells.top()->index(sv->vtx()) );
|
||||
active_contours.push()=sv;
|
||||
return MsgOk;
|
||||
}
|
||||
else
|
||||
{ SR_TRACE1 ( "XXXXXXXXXXXXXXXXXX undef situation!! XXXXXXXXXXXXXXXXXXXXXXXXXXXX" );
|
||||
SR_TRACE_CONTOUR(active_contours.top(),shells.top());
|
||||
SR_TRACE1 ( "s = ["<<a<<","<<b<<"] v="<<e->v);
|
||||
SR_TRACE1 ( "s.fac="<<(int)s->fac()<<", vfac="<<(int)vtxsymedges[e->v]->fac() );
|
||||
}
|
||||
}
|
||||
else
|
||||
{ // e not in the edge tree: input data is not fully connected!
|
||||
}
|
||||
|
||||
s = s->nxt();
|
||||
if ( s==ini ) break;// already passed all edges of the active contour
|
||||
}
|
||||
|
||||
// At this point we looked into all edges of the active_contour,
|
||||
// but no faces were glued and the tree is not empty.
|
||||
|
||||
SR_TRACE1 ( "No more faces could be glued to the actual contour!" );
|
||||
contours_to_solve.push() = active_contours.pop();
|
||||
|
||||
return MsgOk;
|
||||
}
|
||||
|
||||
//============================== virtuals ==================================
|
||||
|
||||
void SeMeshImport::attach_vtx_info ( SeVertex* /*v*/, int /*vi*/ )
|
||||
{
|
||||
}
|
||||
|
||||
void SeMeshImport::attach_edg_info ( SeEdge* /*e*/, int /*a*/, int /*b*/ )
|
||||
{
|
||||
}
|
||||
|
||||
void SeMeshImport::attach_fac_info ( SeFace* /*f*/, int /*fi*/ )
|
||||
{
|
||||
}
|
||||
|
||||
void SeMeshImport::attach_hole_info ( SeFace* /*f*/ )
|
||||
{
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
81
source/dcdt/se/se_mesh_import.h
Normal file
81
source/dcdt/se/se_mesh_import.h
Normal file
@ -0,0 +1,81 @@
|
||||
|
||||
# ifndef SE_MESH_IMPORT_H
|
||||
# define SE_MESH_IMPORT_H
|
||||
|
||||
/** \file sr_mesh_import.h
|
||||
* Topology recovery from a list of triangles
|
||||
*/
|
||||
|
||||
# include "se_mesh.h"
|
||||
|
||||
/*! \class SeMeshImport sr_mesh_import.h
|
||||
\brief Recovers adjacencies from a triangle list
|
||||
Note: SeMeshImport uses the indexing methods of SeMesh. */
|
||||
class SeMeshImport
|
||||
{ public :
|
||||
enum Msg { MsgOk,
|
||||
MsgNullShell,
|
||||
MsgNonManifoldFacesLost,
|
||||
MsgFinished
|
||||
};
|
||||
|
||||
/*! This array will contain the recovered shells, and the user is reuired to
|
||||
get and take control of the stored pointers and after that, putting the size
|
||||
of the array to 0. If the array size is not 0, the destructor of SeMeshImport
|
||||
will delete each shell in the array. */
|
||||
SrArray<SeMeshBase*> shells;
|
||||
|
||||
private :
|
||||
class Data;
|
||||
Data *_data;
|
||||
|
||||
public :
|
||||
/*! Default constructor */
|
||||
SeMeshImport ();
|
||||
|
||||
/*! The destructor will delete all meshes stored in the shells array.
|
||||
So that if the user wants to become responsible to maintain the
|
||||
imported meshes, he/she must take the pointers in the shell array
|
||||
and set the array size to 0. */
|
||||
virtual ~SeMeshImport ();
|
||||
|
||||
/*! Compress some used internal buffers */
|
||||
void compress_buffers ();
|
||||
|
||||
/*! Defines the information describing the triangle-based description to
|
||||
be imported. The list of triangles contains indices to the vertices.
|
||||
Geometrical information can be attached to the mesh by re-writing
|
||||
the availble virtual methods, which allow transforming indices
|
||||
into any kind of user-related data. */
|
||||
Msg start ( const int *triangles, int numtris, int numvtx );
|
||||
|
||||
/*! Join another face to the mesh, expanding the current active contour.
|
||||
This method should be called until MsgFinished or errors occur */
|
||||
Msg next_step ();
|
||||
|
||||
public :
|
||||
|
||||
/* This virtual method must be derived to return a valid SeMesh for
|
||||
each new shell encountered in the data being analysed. */
|
||||
virtual SeMeshBase* get_new_shell ()=0;
|
||||
|
||||
/*! Optional virtual method to attach vertex information.
|
||||
The default implementation does nothing. */
|
||||
virtual void attach_vtx_info ( SeVertex* v, int vi );
|
||||
|
||||
/*! Optional virtual method to attach edge information.
|
||||
The default implementation does nothing. */
|
||||
virtual void attach_edg_info ( SeEdge* e, int a, int b );
|
||||
|
||||
/*! Optional virtual method to attach face information.
|
||||
The default implementation does nothing. */
|
||||
virtual void attach_fac_info ( SeFace* f, int fi );
|
||||
|
||||
/*! Optional virtual method to attach face information of holes found.
|
||||
The default implementation does nothing. */
|
||||
virtual void attach_hole_info ( SeFace* f );
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SE_MESH_IMPORT_H
|
||||
219
source/dcdt/se/se_mesh_io.cpp
Normal file
219
source/dcdt/se/se_mesh_io.cpp
Normal file
@ -0,0 +1,219 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include <string.h>
|
||||
# include "se_mesh.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // IO trace
|
||||
# include "sr_trace.h"
|
||||
|
||||
//================================ IO =========================================
|
||||
|
||||
# define ID(se) int(se->_edge)
|
||||
|
||||
void SeMeshBase::_elemsave ( SrOutput& out, SeMeshBase::ElemType type, SeElement* first )
|
||||
{
|
||||
SrClassManagerBase* man;
|
||||
SeElement *e;
|
||||
int esize;
|
||||
char* st;
|
||||
|
||||
if ( type==TypeVertex )
|
||||
{ esize=_vertices; man=_vtxman; st="Vertices"; }
|
||||
else if ( type==TypeEdge )
|
||||
{ esize=_edges; man=_edgman; st="Edges"; }
|
||||
else
|
||||
{ esize=_faces; man=_facman; st="Faces"; }
|
||||
|
||||
out << srnl << st << srspc << esize << srnl;
|
||||
//fprintf ( f, "\n%s %d\n", st, esize );
|
||||
|
||||
e = first;
|
||||
do { e->_index = 0;
|
||||
out << ID(e->_symedge) <<srspc;
|
||||
//fprintf ( f, "%d ", ID(e->_symedge) );
|
||||
man->output(out,e);
|
||||
out << srnl;
|
||||
//fprintf ( f, "\n" );
|
||||
e = e->nxt();
|
||||
} while (e!=first);
|
||||
|
||||
SR_TRACE1 ( esize << " elements written !" );
|
||||
}
|
||||
|
||||
bool SeMeshBase::save ( SrOutput& out )
|
||||
{
|
||||
SeBase *se;
|
||||
SeElement *el, *eli;
|
||||
int symedges, i, j;
|
||||
|
||||
out << "SYMEDGE MESH DESCRIPTION\n\n";
|
||||
//fprintf ( f, "SYMEDGE MESH DESCRIPTION\n\n" );
|
||||
SR_TRACE1("Header written.");
|
||||
|
||||
symedges = _edges*2;
|
||||
out << "SymEdges " << symedges << srnl;
|
||||
//fprintf ( f, "SymEdges %d\n", symedges );
|
||||
|
||||
if ( empty() ) return true;
|
||||
|
||||
SrArray<SeBase*> ses(symedges);
|
||||
SrArray<SeEdge*> edg(symedges);
|
||||
|
||||
// get adjacency information of symedges to save:
|
||||
SR_TRACE1("Mounting S array...");
|
||||
i=0;
|
||||
el = eli = _first->edg();
|
||||
do { se = el->se();
|
||||
el->_index = i/2;
|
||||
for ( j=0; j<2; j++ )
|
||||
{ if ( j==1 ) se = se->sym();
|
||||
ses[i] = se;
|
||||
edg[i] = se->_edge; // save edge pointer
|
||||
se->_edge = (SeEdge*)i; // and use it to keep indices
|
||||
i++;
|
||||
}
|
||||
el = el->nxt();
|
||||
} while ( el!=eli );
|
||||
|
||||
// adjust vtx and fac indices:
|
||||
SR_TRACE1("Adjusting indexes...");
|
||||
|
||||
i = 0;
|
||||
el = eli = _first->vtx();
|
||||
do { el->_index=i++; el=el->nxt(); } while (el!=eli);
|
||||
|
||||
i = 0;
|
||||
el = eli = _first->fac();
|
||||
do { el->_index=i++; el=el->nxt(); } while (el!=eli);
|
||||
|
||||
for ( i=0; i<ses.size(); i++ )
|
||||
{ out << ID(ses[i]->_next) << srspc
|
||||
<< ID(ses[i]->_rotate) << srspc
|
||||
<< ses[i]->_vertex->_index << srspc
|
||||
<< edg[i]->_index << srspc
|
||||
<< ses[i]->_face->_index << srspc
|
||||
<< srnl;
|
||||
/*fprintf ( f, "%d %d %d %d %d\n",
|
||||
ID(ses[i]->_next),
|
||||
ID(ses[i]->_rotate),
|
||||
ses[i]->_vertex->_index,
|
||||
edg[i]->_index,
|
||||
ses[i]->_face->_index );*/
|
||||
}
|
||||
|
||||
SR_TRACE1("Symedges written.");
|
||||
|
||||
_elemsave ( out, TypeVertex, _first->vtx() );
|
||||
_elemsave ( out, TypeEdge, edg[0] );
|
||||
_elemsave ( out, TypeFace, _first->fac() );
|
||||
|
||||
_curmark = 1;
|
||||
_marking = _indexing = false;
|
||||
for ( i=0; i<ses.size(); i++ ) ses[i]->_edge = edg[i];
|
||||
|
||||
SR_TRACE1 ( "save OK !" );
|
||||
return true;
|
||||
}
|
||||
|
||||
# undef ID
|
||||
|
||||
//---------------------------------- load --------------------------------
|
||||
|
||||
/*
|
||||
static int load_int ( FILE* f )
|
||||
{
|
||||
int i;
|
||||
fscanf ( f, "%d", &i );
|
||||
return i;
|
||||
}
|
||||
|
||||
static void skip ( FILE* f, int n )
|
||||
{
|
||||
char s[60];
|
||||
while ( n-- ) fscanf ( f, "%s", s );
|
||||
}
|
||||
*/
|
||||
void SeMeshBase::_elemload ( SrInput& inp, SrArray<SeElement*>& E,
|
||||
const SrArray<SeBase*>& S, SrClassManagerBase* man )
|
||||
{
|
||||
int i, x;
|
||||
|
||||
inp.get_token(); // skip elem type label
|
||||
//skip ( f, 1 ); // skip elem type label
|
||||
|
||||
inp >> x; E.size(x);
|
||||
//x = load_int(f); E.size(x);
|
||||
|
||||
for ( i=0; i<E.size(); i++ )
|
||||
{ inp >> x; //x = load_int(f);
|
||||
E[i] = (SeElement*)man->alloc();
|
||||
E[i]->_symedge = S.get(x);
|
||||
man->input ( inp, E[i] );
|
||||
//man->read ( E[i], f );
|
||||
if ( i>0 ) E[0]->_insert(E[i]);
|
||||
}
|
||||
|
||||
SR_TRACE1 ( e.size() << " elements loaded !" );
|
||||
}
|
||||
|
||||
bool SeMeshBase::load ( SrInput& inp )
|
||||
{
|
||||
//char buf[64];
|
||||
int i, x;
|
||||
|
||||
//fscanf ( f, "%s", buf ); if ( strcmp(buf,"SYMEDGE") ) return false;
|
||||
//fscanf ( f, "%s", buf ); if ( strcmp(buf,"MESH") ) return false;
|
||||
//fscanf ( f, "%s", buf ); if ( strcmp(buf,"DESCRIPTION") ) return false;
|
||||
inp.get_token(); if ( inp.last_token()!="SYMEDGE" ) return false;
|
||||
inp.get_token(); if ( inp.last_token()!="MESH" ) return false;
|
||||
inp.get_token(); if ( inp.last_token()!="DESCRIPTION" ) return false;
|
||||
SR_TRACE1 ( "Signature ok." );
|
||||
|
||||
// destroy actual structure to load the new one:
|
||||
destroy ();
|
||||
|
||||
// load indices and store them in an array:
|
||||
inp.get_token(); // skip SymEdges label
|
||||
//skip ( f, 1 ); // skip SymEdges label
|
||||
inp >> i; //i = load_int ( f );
|
||||
SrArray<SeBase*> S(i);
|
||||
for ( i=0; i<S.size(); i++ )
|
||||
{ S[i] = new SeBase;
|
||||
inp>>x; S[i]->_next = (SeBase*)x;
|
||||
inp>>x; S[i]->_rotate = (SeBase*)x;
|
||||
inp>>x; S[i]->_vertex = (SeVertex*)x;
|
||||
inp>>x; S[i]->_edge = (SeEdge*)x;
|
||||
inp>>x; S[i]->_face = (SeFace*)x;
|
||||
}
|
||||
SR_TRACE1 ( "Symedges loaded." );
|
||||
|
||||
// load infos:
|
||||
SrArray<SeElement*> V;
|
||||
SrArray<SeElement*> E;
|
||||
SrArray<SeElement*> F;
|
||||
_elemload ( inp, V, S, _vtxman );
|
||||
_elemload ( inp, E, S, _edgman );
|
||||
_elemload ( inp, F, S, _facman );
|
||||
|
||||
// convert indices to pointers:
|
||||
for ( i=0; i<S.size(); i++ )
|
||||
{ S[i]->_next = S[(int)(S[i]->_next)];
|
||||
S[i]->_rotate = S[(int)(S[i]->_rotate)];
|
||||
S[i]->_vertex = V[(int)(S[i]->_vertex)];
|
||||
S[i]->_edge = E[(int)(S[i]->_edge)];
|
||||
S[i]->_face = F[(int)(S[i]->_face)];
|
||||
}
|
||||
|
||||
// adjust internal variables:
|
||||
_first = S[0];
|
||||
_vertices = V.size();
|
||||
_edges = E.size();
|
||||
_faces = F.size();
|
||||
_curmark = 1;
|
||||
_marking = _indexing = false;
|
||||
|
||||
SR_TRACE1 ( "load OK !" );
|
||||
return true;
|
||||
}
|
||||
|
||||
//=== End of File ===================================================================
|
||||
571
source/dcdt/se/se_mesh_operators.cpp
Normal file
571
source/dcdt/se/se_mesh_operators.cpp
Normal file
@ -0,0 +1,571 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include <stdlib.h>
|
||||
# include "se_mesh.h"
|
||||
|
||||
//# define CHECKALL_OPS // Will call check_all() after each operator
|
||||
//# define SR_USE_TRACE1 // Operators trace
|
||||
# include "sr_trace.h"
|
||||
|
||||
//===================================== Some Macros =======================================
|
||||
|
||||
# ifdef CHECKALL_OPS
|
||||
# include <stdlib.h>
|
||||
# define CHECKALL { int err=check_all(); if(err!=0) { printf("CHECK ERROR: %d\n",err ); exit(1); } }
|
||||
# else
|
||||
# define CHECKALL
|
||||
# endif
|
||||
|
||||
# define SE_MAX3(a,b,c) (a>b? (a>c?(a):(c)):(b>c?(b):(c)))
|
||||
|
||||
# define EDG_CREATE(x,y) x=new SeBase; y=new SeBase; \
|
||||
x->_next=y; x->_rotate=x; \
|
||||
y->_next=x; y->_rotate=y;
|
||||
|
||||
# define DEF_SPLICE_VARS SeBase *sp_xnxt, *sp_ynxt, *sp_xsym
|
||||
|
||||
// the effect is: swap(x.nxt,y.nxt); swap(x.nxt.rot,y.nxt.rot)
|
||||
# define SPLICE(x,y) sp_xnxt = x->_next; \
|
||||
sp_ynxt = y->_next; \
|
||||
sp_xsym = sp_xnxt->_rotate; \
|
||||
x->_next = sp_ynxt; \
|
||||
y->_next = sp_xnxt; \
|
||||
sp_xnxt->_rotate = sp_ynxt->_rotate; \
|
||||
sp_ynxt->_rotate = sp_xsym
|
||||
|
||||
//======================================== private members ========================================
|
||||
|
||||
void SeMeshBase::_new_vtx ( SeBase *s )
|
||||
{
|
||||
_vertices++;
|
||||
SeVertex* v = (SeVertex*)_vtxman->alloc();
|
||||
v->_symedge = s;
|
||||
if ( _first ) _first->vtx()->_insert(v);
|
||||
s->_vertex = v;
|
||||
}
|
||||
|
||||
void SeMeshBase::_new_edg ( SeBase *s )
|
||||
{
|
||||
_edges++;
|
||||
SeEdge* e = (SeEdge*)_edgman->alloc();
|
||||
e->_symedge = s;
|
||||
if ( _first ) _first->edg()->_insert(e);
|
||||
s->_edge = e;
|
||||
}
|
||||
|
||||
void SeMeshBase::_new_fac ( SeBase *s )
|
||||
{
|
||||
_faces++;
|
||||
SeFace* f = (SeFace*)_facman->alloc();
|
||||
f->_symedge = s;
|
||||
if ( _first ) _first->fac()->_insert(f);
|
||||
s->_face = f;
|
||||
}
|
||||
|
||||
void SeMeshBase::_del_vtx ( SeElement *e )
|
||||
{
|
||||
_vertices--;
|
||||
if ( e==_first->vtx() ) _first = e->nxt()==e? 0:e->nxt()->se();
|
||||
e->_remove();
|
||||
_vtxman->free ( e );
|
||||
}
|
||||
|
||||
void SeMeshBase::_del_edg ( SeElement *e )
|
||||
{
|
||||
_edges--;
|
||||
if ( e==_first->edg() ) _first = e->nxt()==e? 0:e->nxt()->se();
|
||||
e->_remove();
|
||||
_edgman->free ( e );
|
||||
}
|
||||
|
||||
void SeMeshBase::_del_fac ( SeElement *e )
|
||||
{
|
||||
_faces--;
|
||||
if ( e==_first->fac() ) _first = e->nxt()==e? 0:e->nxt()->se();
|
||||
e->_remove();
|
||||
_facman->free ( e );
|
||||
}
|
||||
|
||||
//==================================== operator related funcs ======================================
|
||||
|
||||
char* SeMeshBase::translate_op_msg ( OpMsg m )
|
||||
{
|
||||
switch (m)
|
||||
{ case OpNoErrors : return "No Errors occured";
|
||||
case OpMevParmsEqual : return "Parms in mev are equal";
|
||||
case OpMevNotSameVtx : return "Parms in mev are not in the same vertex";
|
||||
case OpMefNotSameFace : return "Parms in mef are not in the same face";
|
||||
case OpKevLastEdge : return "Cannot delete the last edge with kev";
|
||||
case OpKevManyEdges : return "More then one edge in parm vertex in kev";
|
||||
case OpKevOneEdge : return "Vertex of parm has only one edge in kev";
|
||||
case OpKefSameFace : return "Parm in kef must be between two different faces";
|
||||
case OpMgUncompatibleFaces : return "Parms in mg must be adjacent to faces with the same number of vertices";
|
||||
case OpMgSameFace : return "Parms in mg must be, each one, adjacent to a different face";
|
||||
case OpFlipOneFace : return "Parm in flip must be between two different faces";
|
||||
case OpEdgColOneFace : return "Parm in edgcol must be between two different faces";
|
||||
case OpVtxSplitParmsEqual : return "Parms in vtxsplit are equal";
|
||||
case OpVtxSplitNotSameVtx : return "Parms in vtxsplit not in the same vertex";
|
||||
}
|
||||
return "Undefined error code";
|
||||
}
|
||||
|
||||
|
||||
static SeMeshBase::ChkMsg check ( SeBase *x, int max )
|
||||
{
|
||||
int i=0;
|
||||
SeBase *xi = x;
|
||||
x = xi->nxt();
|
||||
while ( x!=xi ) { x=x->nxt(); i++; if(i>max) return SeMeshBase::ChkNxtError; }
|
||||
i=0;
|
||||
x = xi->rot();
|
||||
while ( x!=xi ) { x=x->rot(); i++; if(i>max) return SeMeshBase::ChkRotError; }
|
||||
return SeMeshBase::ChkOk;
|
||||
}
|
||||
|
||||
SeMeshBase::ChkMsg SeMeshBase::check_all () const
|
||||
{
|
||||
ChkMsg m;
|
||||
int i, max = 2*SE_MAX3(_vertices,_edges,_faces); // A very high roof to avoid infinite loop
|
||||
SeElement *e, *ei;
|
||||
|
||||
i = 0;
|
||||
e = ei = _first->vtx();
|
||||
do { if ( e->se()->vtx()!=e || i++>max ) return ChkVtxError;
|
||||
e = e->nxt();
|
||||
} while ( e!=ei );
|
||||
|
||||
i = 0;
|
||||
e = ei = _first->edg();
|
||||
do { if ( e->se()->edg()!=e || i++>max ) return ChkEdgError;
|
||||
e = e->nxt();
|
||||
} while ( e!=ei );
|
||||
|
||||
i = 0;
|
||||
e = ei = _first->fac();
|
||||
do { if ( e->se()->fac()!=e || i++>max ) return ChkFacError;
|
||||
e = e->nxt();
|
||||
} while ( e!=ei );
|
||||
|
||||
e = ei = _first->edg();
|
||||
do { m = check ( e->se(), max );
|
||||
if ( m!=ChkOk ) return m;
|
||||
m = check ( e->se()->sym(), max );
|
||||
if ( m!=ChkOk ) return m;
|
||||
e = e->nxt();
|
||||
} while ( e!=ei );
|
||||
return ChkOk;
|
||||
}
|
||||
|
||||
//======================================== SeMeshBase Operators ========================================
|
||||
|
||||
void SeMeshBase::destroy ()
|
||||
{
|
||||
if (!_first) return;
|
||||
SR_TRACE1("destroy...");
|
||||
|
||||
int i;
|
||||
SeElement *ini, *cur, *curn;
|
||||
|
||||
// Save all symedges in a list:
|
||||
SrArray<SeBase*> S(0,_edges*2);
|
||||
cur = ini = _first->edg();
|
||||
do { S.push() = cur->se();
|
||||
S.push() = cur->se()->sym();
|
||||
cur = cur->nxt();
|
||||
} while ( cur!=ini );
|
||||
|
||||
// delete element lists :
|
||||
SrClassManagerBase* man;
|
||||
for ( i=0; i<3; i++ ) // for each element type
|
||||
{ if ( i==0 )
|
||||
{ ini=_first->vtx(); man=_vtxman; }
|
||||
else if ( i==1 )
|
||||
{ ini=_first->edg(); man=_edgman; }
|
||||
else
|
||||
{ ini=_first->fac(); man=_facman; }
|
||||
|
||||
cur=ini;
|
||||
do { curn = cur->nxt(); // for each element
|
||||
man->free ( cur );
|
||||
cur = curn;
|
||||
} while ( cur!=ini );
|
||||
}
|
||||
|
||||
// delete all symedges:
|
||||
for ( i=0; i<S.size(); i++ ) delete S[i];
|
||||
|
||||
_defaults ();
|
||||
|
||||
SR_TRACE1("Ok.");
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::init ()
|
||||
{
|
||||
SR_TRACE1("init...");
|
||||
destroy ();
|
||||
SeBase *ne1, *ne2;
|
||||
EDG_CREATE ( ne1, ne2 );
|
||||
|
||||
_new_vtx(ne1);
|
||||
_new_vtx(ne2); ne2->vtx()->_insert(ne1->vtx());
|
||||
_new_edg(ne1); ne2->_edge=ne1->edg();
|
||||
_new_fac(ne1); ne2->_face=ne1->fac();
|
||||
|
||||
_first = ne2; // _first must be set only here!
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
return ne2;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::mev ( SeBase *s )
|
||||
{
|
||||
SR_TRACE1("mev...");
|
||||
|
||||
SeBase *ne1, *ne2;
|
||||
EDG_CREATE ( ne1, ne2 );
|
||||
|
||||
ne1->_vertex = s->vtx();
|
||||
_new_vtx(ne2); ne1->_vertex=s->vtx();
|
||||
_new_edg(ne2); ne1->_edge=ne2->edg();
|
||||
ne1->_face = ne2->_face = s->fac();
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( s->pri(), ne2 );
|
||||
|
||||
_op_last_msg = OpNoErrors;
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return ne2;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::mev ( SeBase *s1, SeBase *s2 )
|
||||
{
|
||||
SR_TRACE1("mev2...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( s1==s2 ) return _op_error(OpMevParmsEqual);
|
||||
if ( s1->vtx()!=s2->vtx() ) return _op_error(OpMevNotSameVtx);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase *ne1, *ne2;
|
||||
EDG_CREATE ( ne1, ne2 );
|
||||
|
||||
_new_vtx(ne1); ne2->_vertex=s2->vtx();
|
||||
_new_edg(ne1); ne2->_edge = ne1->edg();
|
||||
ne1->_face = s1->sym()->fac();
|
||||
ne2->_face = s2->sym()->fac();
|
||||
s2->_vertex->_symedge = s2;
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( s1->sym(), ne2 );
|
||||
SPLICE ( s2->sym(), ne1 );
|
||||
SPLICE ( ne1, ne2 );
|
||||
|
||||
s2=s1;
|
||||
while ( s1!=ne1 )
|
||||
{ s1->_vertex = ne1->vtx();
|
||||
s1=s1->rot();
|
||||
if(s1==s2) { printf("MEV2 INFINITE LOOP FOUND!\n"); break; }
|
||||
// Prevents infinite loop by incoherent args
|
||||
}
|
||||
|
||||
CHECKALL;
|
||||
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return ne1;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::mef ( SeBase *s1, SeBase *s2 )
|
||||
{
|
||||
SR_TRACE1("mef...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( s1->fac()!=s2->fac() ) return _op_error (OpMefNotSameFace);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase *ne1, *ne2;
|
||||
EDG_CREATE ( ne1, ne2 );
|
||||
|
||||
ne1->_vertex = s2->vtx();
|
||||
ne2->_vertex = s1->vtx();
|
||||
_new_edg(ne1); ne2->_edge=ne1->edg();
|
||||
_new_fac(ne2);
|
||||
ne1->_face=s1->fac(); s1->_face->_symedge=ne1;
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( s1->pri(), ne1 );
|
||||
SPLICE ( s2->pri(), ne2 );
|
||||
|
||||
// s2 = ne2->nxt();
|
||||
while ( s2!=ne2 ) { s2->_face=ne2->fac(); s2=s2->nxt(); }
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return ne2;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::kev ( SeBase *x )
|
||||
{
|
||||
SR_TRACE1("kev...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( _edges==1 ) return _op_error(OpKevLastEdge);
|
||||
if ( x->rot()!=x ) return _op_error(OpKevManyEdges);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase *xn = x->nxt();
|
||||
xn->_vertex->_symedge = xn;
|
||||
xn->_face->_symedge = xn;
|
||||
|
||||
_del_vtx ( x->vtx() );
|
||||
_del_edg ( x->edg() );
|
||||
|
||||
SeBase *xsp = x->sym()->pri();
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( xsp, x );
|
||||
delete x->nxt(); delete x;
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return xsp->nxt();
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::kev ( SeBase *x, SeBase **s )
|
||||
{
|
||||
SR_TRACE1("kev2...");
|
||||
|
||||
SeBase *xs = x->sym();
|
||||
SeBase *xp = x->pri();
|
||||
SeBase *xsp = xs->pri();
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( _edges==1 ) return _op_error(OpKevLastEdge);
|
||||
if ( x->rot()==x ) return _op_error(OpKevOneEdge);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
x->nxt()->vtx()->_symedge = x->nxt();
|
||||
x->fac()->_symedge = xp;
|
||||
x->sym()->fac()->_symedge = xsp;
|
||||
|
||||
_del_vtx ( x->vtx() );
|
||||
_del_edg ( x->edg() );
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( x, xs );
|
||||
SPLICE ( xsp, x );
|
||||
SPLICE ( xp, xs );
|
||||
|
||||
delete x->nxt(); delete x;
|
||||
|
||||
x=xp=xp->sym(); xs=xsp->sym();
|
||||
|
||||
while ( x!=xs )
|
||||
{ x->_vertex=xs->vtx();
|
||||
x=x->rot();
|
||||
if(x==xp) break; // Prevents infinite loop by incoherent args
|
||||
}
|
||||
*s=xs;
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return xp;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::kef ( SeBase *x, SeBase **s )
|
||||
{
|
||||
SR_TRACE1("kef...");
|
||||
|
||||
SeBase *xs = x->sym();
|
||||
SeBase *xp = x->pri();
|
||||
SeBase *xsp = xs->pri();
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( x->fac()==xs->fac() ) return _op_error(OpKefSameFace);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
x->fac()->_symedge = xp;
|
||||
xs->fac()->_symedge = xsp;
|
||||
x->vtx()->_symedge = xs->nxt();
|
||||
xs->vtx()->_symedge = x->nxt();
|
||||
|
||||
_del_edg ( x->edg() );
|
||||
_del_fac ( x->fac() );
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( xsp, x );
|
||||
SPLICE ( xp, xs );
|
||||
delete x->nxt(); delete x;
|
||||
|
||||
xp=xp->nxt(); x=xs=xsp->nxt();
|
||||
|
||||
while ( x!=xp ) { x->_face=xp->fac(); x=x->nxt(); }
|
||||
if (s) *s=xs;
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return xp;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::mg ( SeBase *s1, SeBase *s2 ) // elems of s2 will be deleted
|
||||
{
|
||||
SR_TRACE1("mg...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( vertices_in_face(s1)!=vertices_in_face(s2) ) return _op_error(OpMgUncompatibleFaces);
|
||||
if ( s1->fac()==s2->fac() ) return _op_error(OpMgSameFace);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
_del_fac ( s1->fac() );
|
||||
_del_fac ( s2->fac() );
|
||||
|
||||
SrArray<SeBase*> contour(0,256);
|
||||
SeBase *x1, *x2, *s;
|
||||
|
||||
// Not fully tested: it seems that some fixes will be required like vtx->_symedge=vtx, etc...
|
||||
|
||||
x1=s1; x2=s2;
|
||||
do { _del_edg ( x2->edg() );
|
||||
_del_vtx ( x2->vtx() );
|
||||
x2->sym()->_edge = x1->edg();
|
||||
contour.push()=x1; contour.push()=x2;
|
||||
contour.push()=x1->sym(); contour.push()=x2->sym();
|
||||
x1 = x1->nxt();
|
||||
s=x2; do { s->_vertex=x1->vtx(); s=s->rot(); } while (s!=x2);
|
||||
x2 = x2->pri();
|
||||
} while ( x1!=s1 );
|
||||
|
||||
for ( int i=0; i<contour.size(); i+=4 )
|
||||
{ contour[i+2]->_next->_rotate=contour[i+3];
|
||||
contour[i+3]->_next->_rotate=contour[i+2];
|
||||
delete contour[i];
|
||||
delete contour[i+1];
|
||||
}
|
||||
|
||||
CHECKALL;
|
||||
return s;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::flip ( SeBase *x )
|
||||
{
|
||||
SR_TRACE1("flip...");
|
||||
|
||||
SeBase *xs = x->sym();
|
||||
SeBase *xp = x->pri();
|
||||
SeBase *xsp = xs->pri();
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( x->fac()==xs->fac() ) return _op_error(OpFlipOneFace);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
DEF_SPLICE_VARS;
|
||||
SPLICE ( xsp, x );
|
||||
SPLICE ( xp, xs );
|
||||
|
||||
xp = xp->nxt();
|
||||
xsp = xsp->nxt();
|
||||
|
||||
x->_vertex = xp->nxt()->vtx();
|
||||
xs->_vertex = xsp->nxt()->vtx();
|
||||
xsp->_face = xs->fac();
|
||||
xp->_face = x->fac();
|
||||
|
||||
xp->vtx()->_symedge = xp;
|
||||
xsp->vtx()->_symedge = xsp;
|
||||
xsp->fac()->_symedge = xsp;
|
||||
xp->fac()->_symedge = xp;
|
||||
|
||||
SPLICE ( xp, xs );
|
||||
SPLICE ( xsp, x );
|
||||
|
||||
CHECKALL;
|
||||
SR_TRACE1("Ok.");
|
||||
|
||||
return xs;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::ecol ( SeBase *x, SeBase **s )
|
||||
{
|
||||
SR_TRACE1("ecol...");
|
||||
|
||||
SeBase *s1, *s2, *z;
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( x->fac()==x->sym()->fac() ) return _op_error(OpEdgColOneFace);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
s1 = kev ( x, &s2 );
|
||||
s1 = kef ( s1, &z );
|
||||
s2 = kef ( s2->sym()->nxt(), &z );
|
||||
|
||||
*s = s2->rot();
|
||||
|
||||
return s1;
|
||||
}
|
||||
|
||||
SeBase *SeMeshBase::vsplit ( SeBase *s1, SeBase *s2 )
|
||||
{
|
||||
SR_TRACE1("vtxsplit using mev...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ if ( s1==s2 ) return _op_error(OpVtxSplitParmsEqual);
|
||||
if ( s1->vtx()!=s2->vtx() ) return _op_error(OpVtxSplitNotSameVtx);
|
||||
_op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase* s2s = s2->sym();
|
||||
mef ( s2s->nxt(), s2s );
|
||||
s1 = mef ( s1, s1->nxt() );
|
||||
|
||||
return mev ( s1, s2 );
|
||||
}
|
||||
|
||||
SeBase* SeMeshBase::addv ( SeBase* x )
|
||||
{
|
||||
SR_TRACE1("addv...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ _op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase *y = mev ( x );
|
||||
|
||||
x=x->nxt();
|
||||
do { y = mef ( y, x );
|
||||
x=x->nxt();
|
||||
} while ( x->nxt()!=y );
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
SeBase* SeMeshBase::delv ( SeBase* y )
|
||||
{
|
||||
SR_TRACE1("delv...");
|
||||
|
||||
if ( _op_check_parms )
|
||||
{ _op_last_msg = OpNoErrors;
|
||||
}
|
||||
|
||||
SeBase *s2;
|
||||
|
||||
while ( y!=y->rot() ) y = kef ( y, &s2 );
|
||||
|
||||
return kev ( y );
|
||||
}
|
||||
|
||||
//============================ End of File ===============================
|
||||
1446
source/dcdt/se/se_triangulator.cpp
Normal file
1446
source/dcdt/se/se_triangulator.cpp
Normal file
File diff suppressed because it is too large
Load Diff
366
source/dcdt/se/se_triangulator.h
Normal file
366
source/dcdt/se/se_triangulator.h
Normal file
@ -0,0 +1,366 @@
|
||||
|
||||
# ifndef SE_TRIANGULATOR_H
|
||||
# define SE_TRIANGULATOR_H
|
||||
|
||||
/** \file se_triangulator.h
|
||||
* A triangulator based on symedges
|
||||
*/
|
||||
|
||||
# include "sr_polygon.h"
|
||||
# include "se_mesh.h"
|
||||
|
||||
/*! SeTriangulatorManager contains virtual methods that the user needs to provide
|
||||
to the triangulator, allowing the triangulator to:
|
||||
1.access the coordinates of a vertex, which is a user-denided type.
|
||||
2.notify the user of some events: vertices, edges created, etc.
|
||||
3.access the constrained information of edges. This information should be
|
||||
stored as an array with the constraints ids sharing the constrained edge.
|
||||
However, if constraints are guaranteed to not overllap, the user can
|
||||
simply maintain a boolean variable.
|
||||
Other utility methods are available in the class.
|
||||
Note that the required virtual methods to be implemented are only those
|
||||
related to the triangulator functions being used.
|
||||
*/
|
||||
class SeTriangulatorManager : public SrSharedClass
|
||||
{ public :
|
||||
|
||||
/*! Allows retrieving the coordinates of a vertex. This method is pure
|
||||
virtual, so it needs to be implemented in all cases. */
|
||||
virtual void get_vertex_coordinates ( const SeVertex* v, double& x, double& y )=0;
|
||||
|
||||
/*! Allows setting the coordinates of a vertex. This method is pure
|
||||
virtual, so it needs to be implemented in all cases. */
|
||||
virtual void set_vertex_coordinates ( SeVertex* v, double x, double y )=0;
|
||||
|
||||
/*! This method is called by SeTriangulator::triangulate_face() to notify
|
||||
new edges created during the triangulation process, before the optimization
|
||||
phase, which is based on edge swap. The default implementation simply does nothing.*/
|
||||
virtual void triangulate_face_created_edge ( SeEdge* e );
|
||||
|
||||
/*! This method is called by the triangulator to notify when vertices which
|
||||
are intersection of two constrained edges are inserted in the triangulation.
|
||||
The default implementation simply does nothing. */
|
||||
virtual void new_intersection_vertex_created ( SeVertex* v );
|
||||
|
||||
/*! This method is called by the triangulator to notify when steiner vertices are
|
||||
inserted when recovering conforming line constraints. The default implementation
|
||||
simply does nothing. */
|
||||
virtual void new_steiner_vertex_created ( SeVertex* v );
|
||||
|
||||
/*! This method is called to track vertices which are found when inserting
|
||||
edge constraints with insert_line_constraint(). Meaning that the found
|
||||
vertices v which are in the segment v1, v2 are passed here. The default
|
||||
implementation simply does nothing. */
|
||||
virtual void vertex_found_in_constrained_edge ( SeVertex* v );
|
||||
|
||||
/*! Should return true if the passed edge is constrained. The default
|
||||
implementation always returns false. */
|
||||
virtual bool is_constrained ( SeEdge* e );
|
||||
|
||||
/*! Requires that the edge becomes unconstrained. This is called during
|
||||
manipulation of the triangulation, when edges need to be temporary
|
||||
unconstrained for consistent edge flipping. The default implementation
|
||||
does nothing. */
|
||||
virtual void set_unconstrained ( SeEdge* e );
|
||||
|
||||
/*! Used to see if an edge is constrained. The passed array of ids is empty.
|
||||
If it is returned as empty, it means that the edge is not constrained;
|
||||
and this is the case of the default implementation. */
|
||||
virtual void get_constraints ( SeEdge* e, SrArray<int>& ids );
|
||||
|
||||
/*! This method is called when the edge needs to be set as constrained.
|
||||
The passed array contains the ids to be added to the edge. So that
|
||||
the user can keep track of many constraints sharing the same constrained
|
||||
edge. The passed array will never be empty. The default implementation
|
||||
does nothing. */
|
||||
virtual void add_constraints ( SeEdge* e, const SrArray<int>& ids );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_ccw() */
|
||||
bool ccw ( SeVertex* v1, SeVertex* v2, SeVertex* v3 );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_in_triangle() */
|
||||
bool in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, SeVertex* v );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_in_triangle() */
|
||||
bool in_triangle ( SeVertex* v1, SeVertex* v2, SeVertex* v3, double x, double y );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_in_segment() */
|
||||
bool in_segment ( SeVertex* v1, SeVertex* v2, SeVertex* v, double eps );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_in_circle() */
|
||||
bool is_delaunay ( SeEdge* e );
|
||||
|
||||
/*! Uses sr_ccw() and sr_in_circle() */
|
||||
bool is_flippable_and_not_delaunay ( SeEdge* e );
|
||||
|
||||
/*! Check if (x,y) lies in the interior, edge, or vertex of (v1,v2,v3).
|
||||
A SeTriangulator::LocateResult enum is returned, and s will be adjacent to
|
||||
the found element, if any (sr_next() and sr_in_segment() are used). */
|
||||
int test_boundary ( SeVertex* v1, SeVertex* v2, SeVertex* v3,
|
||||
double x, double y, double eps, SeBase*& s );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_segments_intersect() */
|
||||
bool segments_intersect ( SeVertex* v1, SeVertex* v2,
|
||||
SeVertex* v3, SeVertex* v4, double& x, double& y );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_segments_intersect() */
|
||||
bool segments_intersect ( SeVertex* v1, SeVertex* v2, SeVertex* v3, SeVertex* v4 );
|
||||
|
||||
/*! Retrieves the coordinates and call sr_segments_intersect() */
|
||||
bool segments_intersect ( double x1, double y1, double x2, double y2,
|
||||
SeVertex* v3, SeVertex* v4 );
|
||||
|
||||
/*! Returns the midpoint of segment (v1,v2) */
|
||||
void segment_midpoint ( SeVertex* v1, SeVertex* v2, double& x, double& y );
|
||||
};
|
||||
|
||||
/*! \class SeTriangulator sr_triangulator.h
|
||||
\brief Delaunay triangulation methods
|
||||
|
||||
SeTriangulator provides several methods to construct and update
|
||||
triangulations, including Delaunay triangulations with support
|
||||
to constrained and conforming versions.
|
||||
To use it, a SeTriangulatorManager class is required in order
|
||||
to tell how the triangulator can access the data in the user mesh.
|
||||
The triangulator methods were made to be of flexible use for different
|
||||
applications, so that they only perform the basic algorithms work,
|
||||
leaving many decisions to the user. In this way it is left as user
|
||||
responsability to correctly use them. For instance, the associated mesh
|
||||
must be in fact a triangulation with counter-clockwise triangles, etc,
|
||||
otherwise several algorithms will fail.
|
||||
Most of the used techniques are described in the following paper:
|
||||
M. Kallmann, H. Bieri, and D. Thalmann, "Fully Dynamic Constrained
|
||||
Delaunay Triangulations", In Geometric Modelling for Scientific
|
||||
Visualization, G. Brunnett, B. Hamann, H. Mueller, L. Linsen (Eds.),
|
||||
ISBN 3-540-40116-4, Springer-Verlag, Heidelberg, Germany, pp. 241-257, 2003.
|
||||
However, some solutions presented in the paper may not be available in the current
|
||||
version of the code, which favoured to maintain simpler approaches.
|
||||
All used geometric primitives are implemented in sr_geo2.cpp */
|
||||
class SeTriangulator
|
||||
{ public :
|
||||
|
||||
/*! Note: ModeConforming is not robust when several constraints intersect */
|
||||
enum Mode { ModeUnconstrained, ModeConforming, ModeConstrained };
|
||||
|
||||
/*! Used by locate_point() method */
|
||||
enum LocateResult { NotFound, TriangleFound, EdgeFound, VertexFound };
|
||||
|
||||
private :
|
||||
Mode _mode;
|
||||
double _epsilon;
|
||||
SeMeshBase* _mesh;
|
||||
SeTriangulatorManager* _man;
|
||||
SrArray<SeBase*> _buffer;
|
||||
SrArray<int> _ibuffer;
|
||||
struct ConstrElem { SeVertex* v; SeBase* e;
|
||||
void set(SeVertex* a, SeBase* b) {v=a; e=b;}
|
||||
};
|
||||
SrArray<ConstrElem> _elem_buffer;
|
||||
class PathNode;
|
||||
class PathTree;
|
||||
PathTree* _ptree;
|
||||
bool _path_found;
|
||||
SrArray<SeBase*> _channel;
|
||||
double _xi, _yi, _xg, _yg;
|
||||
class FunnelDeque;
|
||||
FunnelDeque* _fdeque;
|
||||
|
||||
public :
|
||||
|
||||
/*! Three modes of triangulations can be created: unconstrained, conforming,
|
||||
or constrained. Depending on the mode some internal methods will
|
||||
behave differently. In particular, for unconstrained triangulations
|
||||
it is not required to reimplement the methods of SeTriangulatorManager
|
||||
dealing with is/set/get/add edge constraints.
|
||||
The documentation of each method should tell which methods are used from
|
||||
both the associated manager and geometric primitives.
|
||||
The required manager should be allocated with operator new and can be shared.
|
||||
The epsilon is used in the geometric primitives. */
|
||||
SeTriangulator ( Mode mode, SeMeshBase* m, SeTriangulatorManager* man, double epsilon );
|
||||
|
||||
/*! The destructor unreferences the associated SeTriangulatorManager,
|
||||
but it does not delete the associated mesh. */
|
||||
~SeTriangulator ();
|
||||
|
||||
/*! Set a new epsilon */
|
||||
void epsilon ( double eps ) { _epsilon=eps; }
|
||||
|
||||
/*! Get the currently used epsilon */
|
||||
double epsilon () const { return _epsilon; }
|
||||
|
||||
/*! Returns the associated mesh pointer */
|
||||
SeMeshBase* mesh () const { return _mesh; }
|
||||
|
||||
/*! Returns the associated manager pointer */
|
||||
SeTriangulatorManager* manager () const { return _man; }
|
||||
|
||||
/*! Set the desired mode */
|
||||
void mode ( Mode m ) { _mode=m; }
|
||||
|
||||
/*! Get the current triangulator mode */
|
||||
Mode mode () const { return _mode; }
|
||||
|
||||
public :
|
||||
|
||||
/*! This method destroys the associated mesh and initializes it as
|
||||
a triangulated square with the given coordinates. Points must
|
||||
be passed in counter clockwise order. The returned SeBase
|
||||
is adjacent to vertex 1, edge (1,2) and triangle (1,2,4).
|
||||
To construct a square with given maximum and minimum coordinates,
|
||||
points should be set as follows (respectivelly):
|
||||
xmin, ymin, xmax, ymin, xmax, ymax, xmin, ymax.
|
||||
Alternatively, the user may initialize the associated mesh directly
|
||||
by calling the construction operators of SeMesh (such as mev and mef).
|
||||
Normally the mesh should be initialized as a triangulated convex polygon. */
|
||||
SeBase* init_as_triangulated_square
|
||||
( double x1, double y1,
|
||||
double x2, double y2,
|
||||
double x3, double y3,
|
||||
double x4, double y4 );
|
||||
|
||||
/*! Triangulates a counter-clockwise (ccw) oriented face. It is the
|
||||
user responsability to provide a face in the correct ccw orientation.
|
||||
False is returned in case the face cannot be triangulated. However,
|
||||
this migth only happen if the face is not simple.
|
||||
It uses the so called "ear" algorithm, that has worst case complexity
|
||||
of O(n^2), but has a very simple implementation.
|
||||
It uses the geometric primitives sr_ccw() and sr_in_triangle() and
|
||||
each time an edge is inserted during the triangulation construction,
|
||||
the method new_edge_created() of the associated manager is called.
|
||||
If optimization is set to true (the default), the algorithm will
|
||||
flip the created edges to satisfy the Delaunay criterium, and for
|
||||
this, the geometric primitive sr_in_circle() is also used. */
|
||||
bool triangulate_face ( SeFace* f, bool optimize=true );
|
||||
|
||||
/*! This method searches for the location containing the given point.
|
||||
The enumerator LocateResult is returned with the result of the
|
||||
query. If the point is coincident to an existing vertex, VertexFound
|
||||
is returned, and result is incident to the vertex.
|
||||
If the point is found on an edge, EdgeFound is returned, and result
|
||||
is incident to the edge. If the point is inside a triangle, result
|
||||
is incident to the found triangle. In the case that the point is
|
||||
not found to be inside the triangulation, NotFound is returned.
|
||||
The algorithm starts with the given iniface, and continously
|
||||
skips to the neighbour triangle which shares an edge separating the
|
||||
current triangle and the point to reach in two different semi spaces.
|
||||
To avoid possible loops, triangle marking is used.
|
||||
The "distance" of the given iniface and the point to search
|
||||
dictates the performance of the algorithm. The agorithm is O(n)
|
||||
(n = number of triangles). In general the pratical performance
|
||||
is much better than O(n). However serious loop problems will occur
|
||||
if topological inconsistencies are detected.
|
||||
This algorithm may fail to find the point if the border of the
|
||||
triangulation is not convex or if non triangular faces exist.
|
||||
It is mainly based on the geometric primitive sr_ccw(). But it also
|
||||
calls methods sr_points_are_equal() and sr_in_segment(), in order to
|
||||
decide if the point is in an existing edge or vertex. */
|
||||
LocateResult locate_point ( const SeFace* iniface,
|
||||
double x, double y, SeBase*& result );
|
||||
|
||||
/*! Insert a point in the given triangle, with the given coordinates,
|
||||
and then continously flips its edges to ensure the Delaunay criterium.
|
||||
The geometric primitive sr_in_circle() is called during this process.
|
||||
The new vertex inserted is returned and will be never 0 (null).
|
||||
Methods new_vertex_created() and new_edge_created() of the associated
|
||||
manager are called for the new three edges and one vertex created.
|
||||
If the triangulator is of type conforming, methods for managing the
|
||||
constraints ids of edges are called, and to maintain the correctness
|
||||
of the triangulation, new vertices might be automatically inserted
|
||||
in a recursive mid-point subdivision way.
|
||||
If the triangulator is of constrained type, points are inserted only
|
||||
at eventual intersection points. */
|
||||
SeVertex* insert_point_in_face ( SeFace* f, double x, double y );
|
||||
|
||||
/*! Insert a point in the given edge, with the given coordinates.
|
||||
It might be a good practice to project the point to the segment
|
||||
defined by the edge before calling this method.
|
||||
After insertion, it continously flips edges to ensure the Delaunay
|
||||
criterium, as in insert_point_in_face */
|
||||
SeVertex* insert_point_in_edge ( SeEdge* e, double x, double y );
|
||||
|
||||
/*! Insert point searchs the location of (x,y) and correclty insert it in case it
|
||||
is located in an edge or face. If a coincident vertex is found it is returned.
|
||||
If iniface is null, the search starts from mesh()->first()->fac().
|
||||
Null is returned in case the point cannot be located. */
|
||||
SeVertex* insert_point ( double x, double y, const SeFace* inifac=0 );
|
||||
|
||||
/*! Removes a vertex from the Delaunay triangulation. It is the user responsibility
|
||||
to guarantee that the vertex being removed is inside a triangulation, and not
|
||||
at the border of a face which is not triangular.
|
||||
This method simply calls SeMesh::delv() and then retriangulates the created
|
||||
non-triangular polygon, so that no special actions are taken concerning
|
||||
constrained edges. */
|
||||
bool remove_vertex ( SeVertex* v );
|
||||
|
||||
/*! Inserts a line constraint to the current Delaunay triangulation. The line
|
||||
is defined by two existing vertices. This method has a different behavior
|
||||
depending on the actual mode of the triangulator:
|
||||
If the triangulator is in unconstrained mode, nothing is done.
|
||||
If the mode is conforming, Steiner points are inserted starting with the
|
||||
middle point of the missing constraint, and recursively inserted by binary
|
||||
partition until the line becomes present in the triangulation.
|
||||
If the triangulator is in constrained mode, Steiner points are only inserted
|
||||
at the intersection of existing constraints, if there are any. Hence,
|
||||
constrained edges do not respect the Delaunay criterium.
|
||||
False is returned if the algorithm fails, what can occur if non ccw or non
|
||||
triangular cells are found, or in unconstrained mode.
|
||||
The id parameter allows the user to keep track of the created constraints,
|
||||
and it is passed to the corresponding manager method.
|
||||
Note that edges may be referenced by several
|
||||
constrained lines in the case overllap occurs, and thats why each edge
|
||||
of the triangulation should maintain an array of constraints ids.
|
||||
Methods new_vertex_created() and vertex_found_in_constrained_edge() of
|
||||
the manager are called to allow tracking the insertion of Steiner points. */
|
||||
bool insert_line_constraint ( SeVertex *v1, SeVertex *v2, int id );
|
||||
|
||||
/*! Inserts the two points with insert_point() and then call insert_line_constraint().
|
||||
Returns the success or failure of the operation */
|
||||
bool insert_segment ( double x1, double y1, double x2, double y2, int id, const SeFace* inifac=0 );
|
||||
|
||||
/*! Search for a sequence of triangles (e.g. a channel) connecting x1,y1 and x2,y2,
|
||||
without crossing edges marked as constrained.
|
||||
If true is returned, the channel and paths inside the channel can be retrieved
|
||||
with methods get_channel(), get_canonical_path(), get_shortest_path().
|
||||
Note that the channel may not be the shortest one available.
|
||||
Parameter iniface is required in order to feed the process of finding
|
||||
the triangle containing the initial point p1. If it is already the triangle
|
||||
containing p1, the search will be solved trivially.
|
||||
If vistest is true (the default if false) a direct line test is performed prior
|
||||
to the path search.
|
||||
The A* algorithm is used, with the simple heuristic "dist(cur_node,goal_node)" */
|
||||
bool search_path ( double x1, double y1, double x2, double y2,
|
||||
const SeFace* iniface, bool vistest=false );
|
||||
|
||||
/*! Returns a reference to the list with the interior edges of the last channel
|
||||
determined by a sussesfull call to search_path */
|
||||
const SrArray<SeBase*>& get_channel_interior_edges () const { return _channel; }
|
||||
|
||||
/*! Returns a polygon describing the current channel, and thus, method find_path must
|
||||
be succesfully called before to determine the channel to consider. */
|
||||
void get_channel_boundary ( SrPolygon& channel );
|
||||
|
||||
/*! Returns the canonical path, which is the path passing through the midpoint of
|
||||
the channel interior edges. Method search_path must be succesfully called before
|
||||
in order to determine the channel. The path is returned as an open polygon. */
|
||||
void get_canonical_path ( SrPolygon& path );
|
||||
|
||||
/*! Returns the shortest path inside the current channel using the funnel algorithm. */
|
||||
void get_shortest_path ( SrPolygon& path );
|
||||
|
||||
private :
|
||||
void _propagate_delaunay ();
|
||||
bool _conform_line ( SeVertex*, SeVertex*, const SrArray<int>& );
|
||||
bool _constrain_line ( SeVertex*, SeVertex*, const SrArray<int>& );
|
||||
void _v_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, SeVertex*& v, SeBase*& e );
|
||||
void _e_next_step ( SeBase* s, SeVertex* v1, SeVertex* v2, SeVertex*& v, SeBase*& e );
|
||||
bool _can_connect ( SeBase* se, SeBase* sv );
|
||||
bool _blocked ( SeBase* s );
|
||||
void _ptree_init ( LocateResult res, SeBase* s, double xi, double yi, double xg, double yg );
|
||||
int _expand_lowest_cost_leaf ();
|
||||
void _funnel_add ( bool intop, SrPolygon& path, const SrPnt2& p );
|
||||
};
|
||||
|
||||
//============================ End of File =================================
|
||||
|
||||
# endif // SE_TRIANGULATOR_H
|
||||
|
||||
219
source/dcdt/se/sr.cpp
Normal file
219
source/dcdt/se/sr.cpp
Normal file
@ -0,0 +1,219 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdio.h>
|
||||
# include <string.h>
|
||||
|
||||
# include "sr.h"
|
||||
|
||||
//================================ sr.cpp ============================================
|
||||
|
||||
float sr_todeg ( float radians )
|
||||
{
|
||||
return 180.0f * radians / float(SR_PI);
|
||||
}
|
||||
|
||||
double sr_todeg ( double radians )
|
||||
{
|
||||
return 180.0 * radians / double(SR_PI);
|
||||
}
|
||||
|
||||
float sr_torad ( float degrees )
|
||||
{
|
||||
return float(SR_PI) * degrees / 180.0f;
|
||||
}
|
||||
|
||||
double sr_torad ( double degrees )
|
||||
{
|
||||
return double(SR_PI) * degrees / 180.0;
|
||||
}
|
||||
|
||||
float sr_trunc ( float x )
|
||||
{
|
||||
return (float) (int) (x);
|
||||
}
|
||||
|
||||
double sr_trunc ( double x )
|
||||
{
|
||||
return (double) (int) (x);
|
||||
}
|
||||
|
||||
float sr_round ( float x )
|
||||
{
|
||||
return (float) (int) ((x>0.0)? (x+0.5f) : (x-0.5f));
|
||||
}
|
||||
|
||||
double sr_round ( double x )
|
||||
{
|
||||
return (double) (int) ((x>0.0)? (x+0.5) : (x-0.5));
|
||||
}
|
||||
|
||||
float sr_floor ( float x )
|
||||
{
|
||||
return (float) (int) ((x>0.0)? x : (x-1.0f));
|
||||
}
|
||||
|
||||
double sr_floor ( double x )
|
||||
{
|
||||
return (double) (int) ((x>0.0)? x : (x-1.0));
|
||||
}
|
||||
|
||||
float sr_ceil ( float x )
|
||||
{
|
||||
return (float) (int) ((x>0.0)? (x+1.0f) : (x));
|
||||
}
|
||||
|
||||
double sr_ceil ( double x )
|
||||
{
|
||||
return (double) (int) ((x>0.0)? (x+1.0) : (x));
|
||||
}
|
||||
|
||||
int sr_sqrt ( int n )
|
||||
{
|
||||
register int i, s=0, t;
|
||||
|
||||
for ( i=15; i>=0; i-- )
|
||||
{ t = ( s | (1<<i) );
|
||||
if (t*t<=n) s=t;
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
int sr_fact ( int x )
|
||||
{
|
||||
if ( x<2 ) return 1;
|
||||
int m = x;
|
||||
while ( --x>1 ) m *= x;
|
||||
return m;
|
||||
}
|
||||
|
||||
int sr_pow ( int b, int e )
|
||||
{
|
||||
if ( e<=0 ) return 1;
|
||||
int pow=b;
|
||||
while ( --e>0 ) pow*=b;
|
||||
return pow;
|
||||
}
|
||||
|
||||
int sr_compare ( const char *s1, const char *s2 )
|
||||
{
|
||||
int c1, c2; // ANSI definition of toupper() uses int types
|
||||
|
||||
while ( *s1 && *s2 )
|
||||
{ c1 = SR_UPPER(*s1);
|
||||
c2 = SR_UPPER(*s2);
|
||||
if ( c1!=c2 ) return c1-c2;
|
||||
s1++; s2++;
|
||||
}
|
||||
if ( !*s1 && !*s2 ) return 0;
|
||||
return !*s1? -1:1;
|
||||
}
|
||||
|
||||
int sr_compare_cs ( const char *s1, const char *s2 )
|
||||
{
|
||||
int c1, c2; // ANSI definition of toupper() uses int types
|
||||
|
||||
while ( *s1 && *s2 )
|
||||
{ c1 = *s1;
|
||||
c2 = *s2;
|
||||
if ( c1!=c2 ) return c1-c2;
|
||||
s1++; s2++;
|
||||
}
|
||||
if ( !*s1 && !*s2 ) return 0;
|
||||
return !*s1? -1:1;
|
||||
}
|
||||
|
||||
int sr_compare ( const char *s1, const char *s2, int n )
|
||||
{
|
||||
int c1, c2; // ANSI definition of toupper() uses int types
|
||||
|
||||
// printf("[%s]<>[%s] (%d)\n",s1,s2,n);
|
||||
|
||||
while ( *s1 && *s2 )
|
||||
{ c1 = SR_UPPER(*s1);
|
||||
c2 = SR_UPPER(*s2);
|
||||
if ( c1!=c2 ) return c1-c2;
|
||||
s1++; s2++; n--;
|
||||
if ( n==0 ) return n; // are equal
|
||||
}
|
||||
if ( !*s1 && !*s2 ) return 0;
|
||||
return !*s1? -1:1;
|
||||
}
|
||||
|
||||
int sr_compare_cs ( const char *s1, const char *s2, int n )
|
||||
{
|
||||
int c1, c2; // ANSI definition of toupper() uses int types
|
||||
|
||||
// printf("[%s]<>[%s] (%d)\n",s1,s2,n);
|
||||
|
||||
while ( *s1 && *s2 )
|
||||
{ c1 = *s1;
|
||||
c2 = *s2;
|
||||
if ( c1!=c2 ) return c1-c2;
|
||||
s1++; s2++; n--;
|
||||
if ( n==0 ) return n; // are equal
|
||||
}
|
||||
if ( !*s1 && !*s2 ) return 0;
|
||||
return !*s1? -1:1;
|
||||
}
|
||||
|
||||
int sr_compare ( const int *i1, const int *i2 )
|
||||
{
|
||||
return *i1-*i2;
|
||||
}
|
||||
|
||||
int sr_compare ( const float *f1, const float *f2 )
|
||||
{
|
||||
return SR_COMPARE(*f1,*f2);
|
||||
}
|
||||
|
||||
int sr_compare ( const double *d1, const double *d2 )
|
||||
{
|
||||
return SR_COMPARE(*d1,*d2);
|
||||
}
|
||||
|
||||
char* sr_string_new ( const char *tocopy )
|
||||
{
|
||||
if ( !tocopy ) return 0;
|
||||
char *s = new char [ strlen(tocopy)+1 ];
|
||||
strcpy ( s, tocopy );
|
||||
return s;
|
||||
}
|
||||
|
||||
char* sr_string_set ( char*& s, const char *tocopy )
|
||||
{
|
||||
delete[] s;
|
||||
if ( !tocopy ) { s=0; return s; }
|
||||
s = new char [ strlen(tocopy)+1 ];
|
||||
strcpy ( s, tocopy );
|
||||
return s;
|
||||
}
|
||||
|
||||
char* sr_string_realloc ( char*& s, int size )
|
||||
{
|
||||
char *news = 0;
|
||||
|
||||
if ( size>0 )
|
||||
{ news = new char[size];
|
||||
news[0] = 0;
|
||||
if ( s )
|
||||
{ int i;
|
||||
for ( i=0; i<size; i++ )
|
||||
{ news[i]=s[i]; if(!s[i]) break; }
|
||||
}
|
||||
news[size-1] = 0;
|
||||
}
|
||||
|
||||
delete []s;
|
||||
s = news;
|
||||
return s;
|
||||
}
|
||||
|
||||
void sr_stdout_to_file ()
|
||||
{
|
||||
freopen ( "stdout.txt", "w", stdout );
|
||||
setbuf ( stdout, NULL );
|
||||
freopen ( "stderr.txt", "w", stderr );
|
||||
setbuf ( stderr, NULL );
|
||||
}
|
||||
|
||||
//=== End of File =======================================================================
|
||||
|
||||
367
source/dcdt/se/sr.h
Normal file
367
source/dcdt/se/sr.h
Normal file
@ -0,0 +1,367 @@
|
||||
|
||||
# ifndef SR_H
|
||||
# define SR_H
|
||||
|
||||
/** \file sr.h
|
||||
* Main header file of the SR toolkit
|
||||
*
|
||||
\code
|
||||
*************************************************************************
|
||||
*
|
||||
* SR - Simulation and Representation Toolkit
|
||||
* (also called the small scene graph toolkit)
|
||||
* Marcelo Kallmann 1995-2004
|
||||
*
|
||||
* This toolkit was develloped as support for research on modelling,
|
||||
* computational geometry, animation and robotics.
|
||||
*
|
||||
* Main features are classes for:
|
||||
* data structures, math, scene graph, meshes,
|
||||
* dynamic constrained delaunay triangulations and articulated characters.
|
||||
*
|
||||
* Support for OpenGL and FLTK libraries is provided in libs srgl and srfl.
|
||||
*
|
||||
* Main design goals are to be simple and flexible when linking with other tools.
|
||||
* Design choices:
|
||||
* - Small number of classes, with clear and independent functionality
|
||||
* - Uses old c standard library dependency, for most portability and efficiency
|
||||
* - Simple scene graph nodes for small scenes, this is not a scene library
|
||||
* - Transformation matrices follow column major OpenGL format
|
||||
* - Angle parameters are in radians, but degrees are used in data files
|
||||
* - float types are normally preferred than double types, but both are used
|
||||
* - Multiple inheritance is avoided when possible
|
||||
* - Templates are used mainly as type-casting to generic classes, when possible
|
||||
* - Most geometric classes are 3D; otherwise a letter is used to indicate
|
||||
* the dimension, e.g.: SrVec2, SrMatn, etc
|
||||
*
|
||||
*************************************************************************
|
||||
*
|
||||
* Nomenclature conventions/examples :
|
||||
*
|
||||
* global functions : sr_time()
|
||||
* global variables : sr_out
|
||||
* global defines : SR_BIT0, SR_USE_TRACE1
|
||||
* global consts : sre, srpi
|
||||
* global typedefs : srbyte, sruint
|
||||
* global enums : srMyEnum
|
||||
* global classes : SrList, SrVec, SrVec2
|
||||
* class public members : a.size(), a.size(3), v.normalize()
|
||||
* class private members : _num_vertices, _size
|
||||
* class enums : class SrClass { enum Msg { MsgOk, MsgError } };
|
||||
* class friends (no prefix) : dist ( const SrVec& x, const SrVec& y )
|
||||
*
|
||||
* If compiling in windows, make sure the _WIN32 macro is defined.
|
||||
*
|
||||
* Macros for the library compilation (which are not defined by default):
|
||||
* - SR_DEF_BOOL : If the compiler does not have 'bool true false' as keywords,
|
||||
* defining this macro will make sr to define them.
|
||||
* - SR_BV_MATH_FLOAT : To change default type to float in sr_bv_math.h
|
||||
*
|
||||
*************************************************************************
|
||||
\endcode */
|
||||
|
||||
// ==================================== Types =================================
|
||||
|
||||
# ifdef _WIN32
|
||||
# define SR_TARGET_WINDOWS //!< Defined if compiled for windows
|
||||
# else
|
||||
# define SR_TARGET_LINUX //!< Defined if not compiled in windows
|
||||
# endif
|
||||
|
||||
# ifdef SR_DEF_BOOL
|
||||
enum bool { false, true }; //!< for old compilers without bool/true/false keywords
|
||||
# endif
|
||||
|
||||
// The following types should be adjusted according to the used system
|
||||
typedef void* srvoidpt; //!< a pointer to a char
|
||||
typedef char* srcharpt; //!< a pointer to a char
|
||||
typedef signed char srchar; //!< 1 byte signed int, from -127 to 128
|
||||
typedef unsigned char srbyte; //!< 1 byte unsigned int, from 0 to 255
|
||||
typedef unsigned short srword; //!< 2 bytes unsigned int, from 0 to 65,535
|
||||
typedef short int srsint; //!< 2 bytes integer, from -32,768 to 32,767
|
||||
typedef unsigned int sruint; //!< 4 bytes unsigned int, from 0 to 4294967295
|
||||
typedef signed int srint; //!< 4 bytes signed integer, from -2147483648 to 2147483647
|
||||
|
||||
/*! Defines a typedef for a generic comparison function in the form:
|
||||
int srcompare ( const void*, const void* ), that is used by data structure classes. */
|
||||
typedef int (*srcompare) ( const void*, const void* );
|
||||
|
||||
/*! Defines a generic comparison function for the type X, to be used as argument
|
||||
for template based classes. Same as: int (*sr_compare) (const X*,const X*) */
|
||||
# define SR_COMPARE_FUNC int (*sr_compare_func) (const X*,const X*)
|
||||
|
||||
// ================================ Some Constants ======================
|
||||
|
||||
const char srnl = '\n'; //!< Contains the newline '\n' char
|
||||
const char srtab = '\t'; //!< Contains the tab '\t' char
|
||||
const char srspc = ' '; //!< Contains the space char
|
||||
|
||||
const float srtiny = 1.0E-6f; //!< 1E-6
|
||||
const float sre = 2.71828182f; //!< 2.7182818
|
||||
const float srpi = 3.14159265f; //!< 3.1415926
|
||||
const float srpidiv2 = 1.57079632f; //!< 1.57079632
|
||||
const float sr2pi = 6.28318530f; //!< 2*pi
|
||||
const float srsqrt2 = 1.41421356f; //!< sqrt(2) = 1.4142135
|
||||
const float srsqrt3 = 1.73205080f; //!< sqrt(3) = 1.7320508
|
||||
const float srsqrt6 = 2.44948974f; //!< sqrt(6) = 2.4494897
|
||||
|
||||
const sruint sruintmax = ((sruint)0)-1; //!< the unsigned int maximum value
|
||||
|
||||
/* floats and doubles have precision of 7 and 15 decimal digits */
|
||||
# define SR_E 2.7182818284590452 //!< 2.71828...
|
||||
# define SR_PI 3.1415926535897932 //!< 3.141592...
|
||||
# define SR_PIDIV2 1.5707963267948966 //!< 1.570796...
|
||||
# define SR_2PI 6.2831853071795864 //!< 2*pi
|
||||
# define SR_SQRT2 1.4142135623730950 //!< sqrt(2) = 1.4142...
|
||||
# define SR_SQRT3 1.7320508075688772 //!< sqrt(3) = 1.7320...
|
||||
# define SR_SQRT6 2.4494897427831780 //!< sqrt(6) = 2.4494...
|
||||
|
||||
// ================================= Macros ==================================
|
||||
|
||||
/*! \def SR_ASSERT
|
||||
The macro SR_ASSERT(exp) is expanded to a code that sends an error message
|
||||
to sr_out and exist the application, using sr_out.fatal_error()). */
|
||||
# define SR_ASSERT(exp) if ( !(exp) ) sr_out.fatal_error("SR_ASSERT failure in %s::%d !\n",__FILE__,__LINE__);
|
||||
|
||||
/*! Macro that puts c in lower case if c is a valid upper case letter. */
|
||||
# define SR_LOWER(c) ( (c)>='A'&&(c)<='Z'? (c)-'A'+'a':(c) )
|
||||
|
||||
/*! Macro that puts c in upper case if c is a valid lower case letter. */
|
||||
# define SR_UPPER(c) ( (c)>='a'&&(c)<='z'? (c)-'a'+'A':(c) )
|
||||
|
||||
/*! Macro that returns 0 if a is equal to b, 1 if a>b, and -1 otherwise. */
|
||||
# define SR_COMPARE(a,b) (a==b)? 0: (a>b)? 1: -1
|
||||
|
||||
/*! Macro that swaps the value of a boolean type. */
|
||||
# define SR_SWAPB(b) b = !b // B from bool
|
||||
|
||||
/*! Macro that swaps the values of a and b using three xor logical operations. */
|
||||
# define SR_SWAPX(a,b) { a^=b; b^=a; a^=b; } // x from xor
|
||||
|
||||
/*! Macro that swaps the values of a and b, using tmp as temporary variable. */
|
||||
# define SR_SWAPT(a,b,tmp) { tmp=a; a=b; b=tmp; }
|
||||
|
||||
/*! Macro that swaps the values of a and b, given that a temporary
|
||||
variable named tmp, of the same type as a and b exists. */
|
||||
# define SR_SWAP(a,b) { tmp=a; a=b; b=tmp; }
|
||||
|
||||
/*! Macro that returns a number multiple of gap, but that is greater or equal to size. */
|
||||
# define SR_SIZE_WITH_GAP(size,gap) ( (gap) * ( ((size)/(gap)) + ((size)%(gap)==0?0:1) ) )
|
||||
|
||||
/*! Macro that makes m to be x, if x is greater than m. */
|
||||
# define SR_UPDMAX(m,x) if((x)>(m)) m=x
|
||||
|
||||
/*! Macro that makes m to be x, if x is smaller than m. */
|
||||
# define SR_UPDMIN(m,x) if((x)<(m)) m=x
|
||||
|
||||
/*! Macro that tests if x is inside the interval [i,s]. */
|
||||
# define SR_BOUNDED(x,i,s) ((i)<=(x) && (x)<=(s))
|
||||
|
||||
/*! Macro that returns x clipped by the interval [i,s]. */
|
||||
# define SR_BOUND(x,i,s) (x)<(i)? (i): (x)>(s)? (s): (x)
|
||||
|
||||
/*! Macro that forces a to be positive by negating it if it is negative. */
|
||||
# define SR_POS(a) if((a)<0) a=-(a)
|
||||
|
||||
/*! Macro that forces a to be negative by negating it if it is positive. */
|
||||
# define SR_NEG(a) if((a)>0) a=-(a)
|
||||
|
||||
/*! Macro that returns x, so that x = a(1-t) + bt. */
|
||||
# define SR_LERP(a,b,t) ((a)*(1-(t))+(b)*(t)) // return x = a(1-t) + bt
|
||||
|
||||
/*! Macro that returns t, so that x = a(1-t) + bt. */
|
||||
# define SR_PARAM(a,b,x) ((x)-(a))/((b)-(a)) // return t : x = a(1-t) + bt
|
||||
|
||||
/*! Macro that truncates x, with an int typecast. */
|
||||
# define SR_TRUNC(x) ( (int) (x) )
|
||||
|
||||
/*! Macro that returns x rounded to the nearest integer. */
|
||||
# define SR_ROUND(x) ( (int) ((x>0)? (x+0.5f):(x-0.5f)) )
|
||||
|
||||
/*! Macro that rounds x to the nearest integer, but to be
|
||||
applied only when x is positive. This macro adds 0.5
|
||||
and does an int typecast. */
|
||||
# define SR_ROUNDPOS(x) ( (int) (x+0.5) )
|
||||
|
||||
/*! Macro that returns the lowest integer of x. */
|
||||
# define SR_FLOOR(x) ( int( ((x)>0)? (x):((x)-1) ) )
|
||||
|
||||
/*! Macro that returns the highest integer of x. */
|
||||
# define SR_CEIL(x) ( int( ((x)>0)? ((x)+1):(x) ) )
|
||||
|
||||
/*! Macro that returns the maximum value of the two arguments. */
|
||||
# define SR_MAX(a,b) ((a)>(b)? (a):(b))
|
||||
|
||||
/*! Macro that returns the maximum value of the three arguments. */
|
||||
# define SR_MAX3(a,b,c) ((a)>(b)? (a>c?(a):(c)):((b)>(c)?(b):(c)))
|
||||
|
||||
/*! Macro that returns the minimum value of the two arguments. */
|
||||
# define SR_MIN(a,b) ((a)<(b)? (a):(b))
|
||||
|
||||
/*! Macro that returns the minimum value of the three arguments. */
|
||||
# define SR_MIN3(a,b,c) ((a)<(b)? ((a)<(c)?(a):(c)):((b)<(c)?(b):(c)))
|
||||
|
||||
/*! Macro that returns the absolute value of x. */
|
||||
# define SR_ABS(x) ((x)>0? (x):-(x))
|
||||
|
||||
/*! Macro that returns |a-b|, that is the distance of two points in the line. */
|
||||
# define SR_DIST(a,b) ( (a)>(b)? ((a)-(b)):((b)-(a)) )
|
||||
|
||||
/*! Macro that tests if the distance between a and b is closer or equal to ds. */
|
||||
# define SR_NEXT(a,b,ds) ( ( (a)>(b)? ((a)-(b)):((b)-(a)) )<=(ds) )
|
||||
|
||||
/*! Macro that tests if the distance between a and 0 is closer or equal to ds. */
|
||||
# define SR_NEXTZ(a,eps) ( (a)>-(eps) && (a)<(eps) ) // z from zero
|
||||
|
||||
/*! Macro that returns -1 if x is negative, 1 if x is positive and 0 if x is zero. */
|
||||
# define SR_SIGN(x) ((x)<0)? -1: ((x)>0)? 1: 0
|
||||
|
||||
/*! Returns the converted angle, from radians to degrees (float version). */
|
||||
# define SR_TODEG(r) (180.0f*float(r)/srpi)
|
||||
|
||||
/*! Returns the converted angle, from degrees to radians (float version). */
|
||||
# define SR_TORAD(d) (srpi*float(d)/180.0f)
|
||||
|
||||
/*! Returns the converted angle, from radians to degrees (double version). */
|
||||
# define SR_TODEGd(r) (180.0*double(r)/SR_PI)
|
||||
|
||||
/*! Returns the converted angle, from degrees to radians (double version). */
|
||||
# define SR_TORADd(d) (SR_PI*double(d)/180.0)
|
||||
|
||||
// ============================== Math Utilities ===========================
|
||||
|
||||
/*! Returns the convertion from radians to degrees (float version). */
|
||||
float sr_todeg ( float radians );
|
||||
|
||||
/*! Returns the convertion from radians to degrees (double version). */
|
||||
double sr_todeg ( double radians );
|
||||
|
||||
/*! Returns the convertion from degrees to radians (float version). */
|
||||
float sr_torad ( float degrees );
|
||||
|
||||
/*! Returns the convertion from degrees to radians (double version). */
|
||||
double sr_torad ( double degrees );
|
||||
|
||||
/*! Returns the integer part of x by using a sequence of type casts (float version). */
|
||||
float sr_trunc ( float x );
|
||||
|
||||
/*! Returns the integer part of x by using a sequence of type casts (double version). */
|
||||
double sr_trunc ( double x );
|
||||
|
||||
/*! Returns the closest integer of x (float version). */
|
||||
float sr_round ( float x );
|
||||
|
||||
/*! Returns the closest integer of x (double version). */
|
||||
double sr_round ( double x );
|
||||
|
||||
/*! Returns the lowest rounded value of x (float version). */
|
||||
float sr_floor ( float x );
|
||||
|
||||
/*! Returns the lowest rounded value of x (double version). */
|
||||
double sr_floor ( double x );
|
||||
|
||||
/*! Returns the highest rounded value of x (float version). */
|
||||
float sr_ceil ( float x );
|
||||
|
||||
/*! Returns the highest rounded value of x (double version). */
|
||||
double sr_ceil ( double x );
|
||||
|
||||
/*! Returns the square root for integer values, with no use of floating point. */
|
||||
int sr_sqrt ( int x );
|
||||
|
||||
/*! sr_fact returns the factorial of x. */
|
||||
int sr_fact ( int x );
|
||||
|
||||
/*! returns "b^e", e must be >=0 */
|
||||
int sr_pow ( int b, int e );
|
||||
|
||||
// ============================= Compare Functions ============================
|
||||
|
||||
/*! Case insensitive comparison of strings in the C style
|
||||
This function follows the C style of compare functions where 0 is returned if
|
||||
s1==s2, <0 if s1<s2, and >0 otherwise. Comparisons are case-insensitive.
|
||||
s1 and s2 must be non-null pointers, otherwise unpredictable results will arise.
|
||||
If two strings have the first n characters equal, where one has lenght n, and
|
||||
the other has length >n, the smaller one is considered to come first. */
|
||||
int sr_compare ( const char *s1, const char *s2 );
|
||||
|
||||
/*! Case sensitive comparison of strings in the C style */
|
||||
int sr_compare_cs ( const char *s1, const char *s2 );
|
||||
|
||||
/*! Case insensitive compare strings, but compares a maximum of n characters. */
|
||||
int sr_compare ( const char *s1, const char *s2, int n );
|
||||
|
||||
/*! Case sensitive compare strings, but compares a maximum of n characters. */
|
||||
int sr_compare_cs ( const char *s1, const char *s2, int n );
|
||||
|
||||
/*! Compares two integers, returning 0 if they're equal, <0 if i1<i2, and >0 otherwise. */
|
||||
int sr_compare ( const int *i1, const int *i2 );
|
||||
|
||||
/*! Compares two floats, returning 0 if they're equal, <0 if f1<f2, and >0 otherwise. */
|
||||
int sr_compare ( const float *f1, const float *f2 );
|
||||
|
||||
/*! Compares two doubles, returning 0 if they're equal, <0 if d1<d2, and >0 otherwise. */
|
||||
int sr_compare ( const double *d1, const double *d2 );
|
||||
|
||||
// ============================== C String Utilities ============================
|
||||
|
||||
/*! Allocates a string with sufficient size to copy 'tocopy' in it.
|
||||
The allocation is simply done with operator new, and the allocated
|
||||
memory pointer is returned. If tocopy==0, the value 0 is returned. */
|
||||
char* sr_string_new ( const char* tocopy );
|
||||
|
||||
/*! Deletes s, and reallocates s with sufficient size to copy 'tocopy'
|
||||
in it. If tocopy==0, s will be a null pointer, and 0 is returned.
|
||||
Otherwise the allocation is simply done with operator new, the
|
||||
allocated memory pointer is returned and s is changed to point to
|
||||
this new memory allocated so that the returned value will be the
|
||||
same as s. */
|
||||
char* sr_string_set ( char*& s, const char *tocopy );
|
||||
|
||||
/*! Deletes s, and reallocates it with the given size also copying its
|
||||
contents to the new allocated position. If size<=0, s is simply
|
||||
deleted. If the new size is smaller then the original s length,
|
||||
the contents will be truncated. In all cases, the new s is returned
|
||||
and will be a valid string, having the ending null char. This
|
||||
function is similar to the C standard realloc function, but using
|
||||
C++ operators new/delete. */
|
||||
char* sr_string_realloc ( char*& s, int size );
|
||||
|
||||
// ============================== Standard IO ============================
|
||||
|
||||
/*! Redirects the C streams stdout and stderr to the text files
|
||||
stdout.txt and stderr.txt in the current folder */
|
||||
void sr_stdout_to_file ();
|
||||
|
||||
// ============================== Bit Operation ============================
|
||||
|
||||
/*! Tests if flg has the given bit set. */
|
||||
# define SR_FLAG_TEST(flg,bit) ((flg)&(bit))
|
||||
|
||||
/*! Sets on the given bit of the flag. */
|
||||
# define SR_FLAG_ON(flg,bit) flg|=bit
|
||||
|
||||
/*! Swaps the given bit of the flag. */
|
||||
# define SR_FLAG_SWAP(flg,bit) flg^=bit
|
||||
|
||||
/*! Sets off the given bit of the flag. This requires two instructions, so
|
||||
that SR_FLAG_SWAP is faster. */
|
||||
# define SR_FLAG_OFF(flg,bit) if((flg)&(bit)) flg^=bit
|
||||
|
||||
/*! Sets the given bit of the flag to be on or off, according to the boolean value of val. */
|
||||
# define SR_FLAG_SET(flg,bit,val) flg = (val)? (flg)|(bit) : ((flg)&(bit)? (flg)^(bit):flg)
|
||||
|
||||
# define SR_BIT0 1 //!< Same as 1
|
||||
# define SR_BIT1 2 //!< Same as 2
|
||||
# define SR_BIT2 4 //!< Same as 4
|
||||
# define SR_BIT3 8 //!< Same as 8
|
||||
# define SR_BIT4 16 //!< Same as 16
|
||||
# define SR_BIT5 32 //!< Same as 32
|
||||
# define SR_BIT6 64 //!< Same as 64
|
||||
# define SR_BIT7 128 //!< Same as 128
|
||||
# define SR_ALLBITS 255 //!< Same as 255
|
||||
# define SR_NOBITS 0 //!< Same as 0
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_H
|
||||
224
source/dcdt/se/sr_alg.cpp
Normal file
224
source/dcdt/se/sr_alg.cpp
Normal file
@ -0,0 +1,224 @@
|
||||
#include "precompiled.h"
|
||||
# include <math.h>
|
||||
|
||||
# include "sr.h"
|
||||
# include "sr_alg.h"
|
||||
|
||||
/*-------------------------------------------------------------------*/
|
||||
/* Functions to solve polynomials of 2nd, 3rt and 4th degree. */
|
||||
/* Source: graphics gems */
|
||||
/*-------------------------------------------------------------------*/
|
||||
|
||||
//change kai:
|
||||
//# define aMAXFLOAT 3.40282347E+38F
|
||||
# define aMAXFLOAT 3.40282347E+28F
|
||||
# define aEPSILON 1e-9
|
||||
# define aEPSILON2 0.00001
|
||||
# define aISZERO(x) ((x) > -aEPSILON && (x) < aEPSILON)
|
||||
# define aCBRT(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
|
||||
((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
|
||||
|
||||
int sr_solve_quadric_polynomial ( double c[3], double s[2] )
|
||||
{
|
||||
double p, q, D;
|
||||
|
||||
// normal form: x^2 + px + q = 0
|
||||
p = c[1] / (2*c[2]);
|
||||
q = c[0] / c[2];
|
||||
|
||||
D = p*p - q;
|
||||
|
||||
if ( aISZERO(D) )
|
||||
{ s[0] = -p;
|
||||
return 1;
|
||||
}
|
||||
else if ( D<0 )
|
||||
{ return 0;
|
||||
}
|
||||
else // if (D > 0)
|
||||
{ double sqrt_D = sqrt(D);
|
||||
s[0] = sqrt_D - p;
|
||||
s[1] = - sqrt_D - p;
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
int sr_solve_cubic_polynomial ( double c[4], double s[3] )
|
||||
{
|
||||
int i, num;
|
||||
double sub;
|
||||
double A, B, C;
|
||||
double sq_A, p, q;
|
||||
double cb_p, D;
|
||||
|
||||
// normal form: x^3 + Ax^2 + Bx + C = 0
|
||||
A = c[2] / c[3];
|
||||
B = c[1] / c[3];
|
||||
C = c[0] / c[3];
|
||||
|
||||
// substitute x = y - A/3 to eliminate quadric term:
|
||||
// x^3 +px + q = 0
|
||||
sq_A = A * A;
|
||||
p = 1.0/3 * (- 1.0/3 * sq_A + B);
|
||||
q = 1.0/2 * (A * (2.0/27 * sq_A - 1.0/3 * B) + C);
|
||||
|
||||
// use Cardano's formula
|
||||
cb_p = p * p * p;
|
||||
D = q * q + cb_p;
|
||||
|
||||
if ( aISZERO(D) )
|
||||
{ if ( aISZERO(q) ) // one triple solution
|
||||
{ s[0] = 0;
|
||||
num = 1;
|
||||
}
|
||||
else // one single and one double solution
|
||||
{ double u = aCBRT(-q);
|
||||
s[0] = 2 * u;
|
||||
s[1] = - u;
|
||||
num = 2;
|
||||
}
|
||||
}
|
||||
else if ( D<0 ) // Casus irreducibilis: three real solutions
|
||||
{ double phi = 1.0/3 * acos ( -q/sqrt(-cb_p) );
|
||||
double t = 2 * sqrt(-p);
|
||||
s[0] = t * cos(phi);
|
||||
s[1] = - t * cos(phi + SR_PI / 3);
|
||||
s[2] = - t * cos(phi - SR_PI / 3);
|
||||
num = 3;
|
||||
}
|
||||
else /* one real solution */
|
||||
{ double sqrt_D = sqrt(D);
|
||||
double u = aCBRT(sqrt_D - q);
|
||||
double v = - aCBRT(sqrt_D + q);
|
||||
s[0] = u + v;
|
||||
num = 1;
|
||||
}
|
||||
|
||||
// resubstitute
|
||||
sub = 1.0/3 * A;
|
||||
for ( i=0; i<num; ++i ) s[i] -= sub;
|
||||
return num;
|
||||
}
|
||||
|
||||
int sr_solve_quartic_polynomial ( double c[5], double s[4] )
|
||||
{
|
||||
double coeffs[4];
|
||||
double z, u, v, sub;
|
||||
double A, B, C, D;
|
||||
double sq_A, p, q, r;
|
||||
int i, num;
|
||||
|
||||
// normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0
|
||||
A = c[3] / c[4];
|
||||
B = c[2] / c[4];
|
||||
C = c[1] / c[4];
|
||||
D = c[0] / c[4];
|
||||
|
||||
// substitute x = y - A/4 to eliminate cubic term:
|
||||
// x^4 + px^2 + qx + r = 0
|
||||
sq_A = A * A;
|
||||
p = - 3.0/8 * sq_A + B;
|
||||
q = A * (1.0/8 * sq_A - 1.0/2 * B) + C;
|
||||
r = sq_A * (- 3.0/256*sq_A + 1.0/16*B) - 1.0/4*A*C + D;
|
||||
|
||||
if ( aISZERO(r) ) // no absolute term: y(y^3 + py + q) = 0
|
||||
{ coeffs[0] = q;
|
||||
coeffs[1] = p;
|
||||
coeffs[2] = 0;
|
||||
coeffs[3] = 1;
|
||||
num = sr_solve_cubic_polynomial ( coeffs, s );
|
||||
s[num++] = 0;
|
||||
}
|
||||
else // solve the resolvent cubic ...
|
||||
{ coeffs[0] = 1.0/2 * r * p - 1.0/8 * q * q;
|
||||
coeffs[1] = - r;
|
||||
coeffs[2] = - 1.0/2 * p;
|
||||
coeffs[3] = 1;
|
||||
sr_solve_cubic_polynomial ( coeffs, s );
|
||||
// ... and take the one real solution ...
|
||||
z = s[ 0 ];
|
||||
// ... to build two quadric equations
|
||||
u = z * z - r;
|
||||
v = 2 * z - p;
|
||||
|
||||
if (aISZERO(u)) u = 0;
|
||||
else if (u > 0) u = sqrt(u);
|
||||
else return 0;
|
||||
|
||||
if (aISZERO(v)) v = 0;
|
||||
else if (v > 0) v = sqrt(v);
|
||||
else return 0;
|
||||
|
||||
coeffs[0] = z - u;
|
||||
coeffs[1] = q < 0 ? -v : v;
|
||||
coeffs[2] = 1;
|
||||
num = sr_solve_quadric_polynomial ( coeffs, s );
|
||||
|
||||
coeffs[0]= z + u;
|
||||
coeffs[1] = q < 0 ? v : -v;
|
||||
coeffs[2] = 1;
|
||||
num += sr_solve_quadric_polynomial ( coeffs, s+num );
|
||||
}
|
||||
|
||||
// resubstitute
|
||||
sub = 1.0/4 * A;
|
||||
for ( i=0; i<num; ++i ) s[i] -= sub;
|
||||
return num;
|
||||
}
|
||||
|
||||
float sr_in_ellipse ( float x, float y, float a, float b )
|
||||
{
|
||||
float cx = x/a;
|
||||
float cy = y/b;
|
||||
return cx*cx + cy*cy - 1;
|
||||
}
|
||||
|
||||
// replaces (x,y) by its closest point on the ellipse (a,b)
|
||||
void sr_get_closest_on_ellipse ( float a, float b, float& x, float& y )
|
||||
{
|
||||
double c[5], s[4];
|
||||
double r = a / b,
|
||||
e = 2. * (b - a*r),
|
||||
f = 2. * (x * r);
|
||||
|
||||
if ( fabs(y)<aEPSILON2 )
|
||||
{ x = x>0 ? a : -a;
|
||||
y = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
c [4] = y;
|
||||
c [3] = f - e;
|
||||
c [2] = 0;
|
||||
c [1] = f + e;
|
||||
c [0] = -y;
|
||||
|
||||
int nb = sr_solve_quartic_polynomial ( c, s );
|
||||
|
||||
double denom, s_theta, c_theta, dist;
|
||||
double test_x [4], test_y [4];
|
||||
double min_dist = aMAXFLOAT;
|
||||
int i, winner=0;
|
||||
|
||||
// Find the closest point
|
||||
for ( i=0; i<nb; i++ )
|
||||
{ denom = 1. + s [i] * s [i];
|
||||
s_theta = 2. * s [i] / denom;
|
||||
c_theta = (1. - s [i] * s [i]) / denom;
|
||||
|
||||
test_x[i] = a * c_theta;
|
||||
test_y[i] = b * s_theta;
|
||||
|
||||
dist = (test_x[i]-x) * (test_x[i]-x) + (test_y[i]-y) * (test_y[i]-y);
|
||||
if ( dist<min_dist )
|
||||
{ min_dist = dist;
|
||||
winner = i;
|
||||
}
|
||||
}
|
||||
|
||||
x = (float)test_x[winner];
|
||||
y = (float)test_y[winner];
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
34
source/dcdt/se/sr_alg.h
Normal file
34
source/dcdt/se/sr_alg.h
Normal file
@ -0,0 +1,34 @@
|
||||
|
||||
/** \file sr_alg.h
|
||||
* few algebra functions */
|
||||
|
||||
# ifndef SR_ALG_H
|
||||
# define SR_ALG_H
|
||||
|
||||
/*! Solve polynomials of 2nd degree. The coefficients are given in c,
|
||||
where c[i] is the coefficient of the i-th power of the unknown variable.
|
||||
The roots are stored in s, and the number of stored roots is returned. */
|
||||
int sr_solve_quadric_polynomial ( double c[3], double s[2] );
|
||||
|
||||
/*! Solve polynomials of 3rd degree. The coefficients are given in c,
|
||||
where c[i] is the coefficient of the i-th power of the unknown variable.
|
||||
The roots are stored in s, and the number of stored roots is returned. */
|
||||
int sr_solve_cubic_polynomial ( double c[4], double s[3] );
|
||||
|
||||
/*! Solve polynomials of 4th degree. The coefficients are given in c,
|
||||
where c[i] is the coefficient of the i-th power of the unknown variable.
|
||||
The roots are stored in s, and the number of stored roots is returned. */
|
||||
int sr_solve_quartic_polynomial ( double c[5], double s[4] );
|
||||
|
||||
/*! Returns <0 if (x,y) is inside the ellipse with radius (a,b), returns
|
||||
0 if it is on the ellipse, and returns >0 if it is outside */
|
||||
float sr_in_ellipse ( float x, float y, float a, float b );
|
||||
|
||||
/*! This function replaces the given point (x,y) by the closest point
|
||||
on the ellipse whose semi-axes are of length a and b. The solution
|
||||
is derived analyticaly, solving a quartic polynomial */
|
||||
void sr_get_closest_on_ellipse ( float a, float b, float& x, float& y );
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_ALG_H
|
||||
189
source/dcdt/se/sr_array.cpp
Normal file
189
source/dcdt/se/sr_array.cpp
Normal file
@ -0,0 +1,189 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdlib.h>
|
||||
# include <string.h>
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
//# define SR_USE_MEM_CONTROL
|
||||
# include "sr_mem_control.h"
|
||||
|
||||
# define DATA(i) ((char*)_data)+(sizeofx*(i))
|
||||
# define NEWDATA(i) ((char*)newdata)+(sizeofx*(i))
|
||||
|
||||
//================================= methods =======================================================
|
||||
|
||||
SrArrayBase::SrArrayBase ( sruint sizeofx, int s, int c )
|
||||
: _size(s), _capacity(c)
|
||||
{
|
||||
if ( _capacity<_size ) _capacity=_size;
|
||||
_data = _capacity>0? SR_MALLOC(sizeofx*_capacity) : 0;
|
||||
}
|
||||
|
||||
SrArrayBase::SrArrayBase ( sruint sizeofx, const SrArrayBase& a )
|
||||
: _size(a._size), _capacity(a._size)
|
||||
{
|
||||
if ( _capacity>0 )
|
||||
{ _data = SR_MALLOC ( sizeofx*_capacity );
|
||||
if ( _size>0 ) memcpy ( _data, a._data, sizeofx*_size );
|
||||
}
|
||||
else _data = 0;
|
||||
}
|
||||
|
||||
SrArrayBase::SrArrayBase ( void* pt, int s, int c )
|
||||
: _data(pt), _size(s), _capacity(c)
|
||||
{
|
||||
}
|
||||
|
||||
void SrArrayBase::free_data ()
|
||||
{
|
||||
if (_data) SR_FREE(_data);
|
||||
}
|
||||
|
||||
void SrArrayBase::size ( unsigned sizeofx, int ns )
|
||||
{
|
||||
_size = ns;
|
||||
if ( _size<0 ) _size=0;
|
||||
|
||||
if ( _size>_capacity )
|
||||
{ _capacity = _size;
|
||||
_data = realloc ( _data, sizeofx*_capacity ); // if _data==0, realloc reacts as malloc.
|
||||
}
|
||||
}
|
||||
|
||||
void SrArrayBase::capacity ( unsigned sizeofx, int nc )
|
||||
{
|
||||
if ( nc<0 ) nc = 0;
|
||||
if ( nc==_capacity ) return;
|
||||
if ( nc==0 ) { if(_data)free(_data); _data=0; _capacity=_size=0; return; }
|
||||
|
||||
_capacity = nc;
|
||||
if ( _size>_capacity ) _size=_capacity;
|
||||
|
||||
_data = realloc ( _data, sizeofx*_capacity ); // if _data==0, realloc reacts as malloc.
|
||||
}
|
||||
|
||||
int SrArrayBase::validate ( int index ) const
|
||||
{
|
||||
if ( index<0 ) index += _size * ( -index/_size + 1 );
|
||||
SR_ASSERT ( index>=0 );
|
||||
return index%_size;
|
||||
}
|
||||
|
||||
void SrArrayBase::compress ( sruint sizeofx )
|
||||
{
|
||||
if ( _size==_capacity ) return;
|
||||
|
||||
if ( !_size ) { SR_FREE ( _data ); _data=0; }
|
||||
else _data = SR_REALLOC ( _data, sizeofx*_size );
|
||||
|
||||
_capacity = _size;
|
||||
}
|
||||
|
||||
void SrArrayBase::remove ( sruint sizeofx, int i, int dp )
|
||||
{
|
||||
if ( i<_size-dp ) memmove ( DATA(i), DATA(i+dp), sizeofx*(_size-(i+dp)) );
|
||||
_size-=dp;
|
||||
}
|
||||
|
||||
void SrArrayBase::insert ( sruint sizeofx, int i, int dp )
|
||||
{
|
||||
SR_ASSERT ( i>=0 && i<=_size);
|
||||
SR_ASSERT ( dp>0 );
|
||||
|
||||
_size += dp;
|
||||
|
||||
if ( _size>_capacity )
|
||||
{ _capacity = _size*2;
|
||||
_data = SR_REALLOC ( _data, sizeofx*_capacity );
|
||||
}
|
||||
|
||||
if ( i<_size-dp )
|
||||
{ memmove ( DATA(i+dp), DATA(i), sizeofx*(_size-dp-i) ); // ok with overlap
|
||||
}
|
||||
}
|
||||
|
||||
void SrArrayBase::move ( sruint sizeofx, int dest, int src, int n )
|
||||
{
|
||||
memmove ( DATA(dest), DATA(src), sizeofx*(n) );
|
||||
}
|
||||
|
||||
void SrArrayBase::sort ( sruint sizeofx, srcompare cmp )
|
||||
{
|
||||
::qsort ( _data, (size_t)_size, (size_t)sizeofx, cmp );
|
||||
}
|
||||
|
||||
int SrArrayBase::lsearch ( sruint sizeofx, const void *x, srcompare cmp ) const
|
||||
{
|
||||
char *pt = (char*)_data;
|
||||
|
||||
for ( int i=0; i<_size; i++ ) { if ( cmp(pt,x)==0 ) return i; pt+=sizeofx; }
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
int SrArrayBase::bsearch ( sruint sizeofx, const void *x, srcompare cmp, int *pos ) const
|
||||
{
|
||||
int comp;
|
||||
int i, e, p;
|
||||
|
||||
comp=1; i=0; e=_size; p=(e-i)/2;
|
||||
|
||||
while ( comp!=0 && i!=e )
|
||||
{ comp = cmp ( x, DATA(p) );
|
||||
if ( comp<0 ) e = p;
|
||||
else if ( comp>0 ) i = p+1;
|
||||
p = i + (e-i)/2;
|
||||
}
|
||||
|
||||
if (pos) *pos=p;
|
||||
|
||||
return comp==0? p : -1;
|
||||
}
|
||||
|
||||
int SrArrayBase::insort ( sruint sizeofx, const void *x, srcompare cmp, bool allowdup )
|
||||
{
|
||||
int result, pos;
|
||||
result = bsearch ( sizeofx, x, cmp, &pos );
|
||||
if ( result!=-1 && !allowdup ) return -1;
|
||||
insert ( sizeofx, pos, 1 );
|
||||
return pos;
|
||||
}
|
||||
|
||||
void SrArrayBase::copy ( sruint sizeofx, const SrArrayBase& a )
|
||||
{
|
||||
if ( _data==a._data ) return;
|
||||
if ( _data ) { SR_FREE(_data); _data=0; }
|
||||
_capacity = a._capacity;
|
||||
_size = a._size;
|
||||
if ( _capacity>0 )
|
||||
{ _data = SR_MALLOC ( sizeofx*_capacity );
|
||||
if ( _size>0 ) memcpy ( _data, a._data, sizeofx*_size );
|
||||
}
|
||||
}
|
||||
|
||||
void* SrArrayBase::leave_data ()
|
||||
{
|
||||
void *pt = _data;
|
||||
_data=0;
|
||||
_size=0;
|
||||
_capacity=0;
|
||||
return pt;
|
||||
}
|
||||
|
||||
void SrArrayBase::take_data ( SrArrayBase& a )
|
||||
{
|
||||
if ( _data ) SR_FREE ( _data );
|
||||
_data = a._data; a._data=0;
|
||||
_size = a._size; a._size=0;
|
||||
_capacity = a._capacity; a._capacity=0;
|
||||
}
|
||||
|
||||
void SrArrayBase::take_data ( void* pt, int s, int c )
|
||||
{
|
||||
if ( _data ) SR_FREE ( _data );
|
||||
_data = pt;
|
||||
_size = s;
|
||||
_capacity = c;
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
341
source/dcdt/se/sr_array.h
Normal file
341
source/dcdt/se/sr_array.h
Normal file
@ -0,0 +1,341 @@
|
||||
|
||||
# ifndef SR_ARRAY_H
|
||||
# define SR_ARRAY_H
|
||||
|
||||
/** \file sr_array.h
|
||||
* fast resizeable array template */
|
||||
|
||||
# include "sr.h"
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
|
||||
/*! \class SrArrayBase sr_array.h
|
||||
\brief Fast resizeable array base class
|
||||
|
||||
This class is to be derived, and not to be directly used. See SrArray for
|
||||
a user-ready class. All memory management functions of SrArrayBase were
|
||||
written using quick memory block functions. In this way, malloc() and free()
|
||||
functions are used. So that void pointers are used to refer to user's data.
|
||||
Most methods need to know the size of each element that is the parameter
|
||||
sizeofx appearing several times. */
|
||||
class SrArrayBase
|
||||
{ protected :
|
||||
|
||||
void* _data; //!< Array pointer used for storage
|
||||
int _size; //!< Number of elements being used in array
|
||||
int _capacity; //!< Number of allocated elements (>=size)
|
||||
|
||||
protected :
|
||||
|
||||
/*! Init with the sizeof of each element, size, and capacity. If the given
|
||||
capacity is smaller than the size, capacity is set to be equal to size */
|
||||
SrArrayBase ( sruint sizeofx, int s, int c );
|
||||
|
||||
/*! Copy constructor. Allocates and copies size elements. */
|
||||
SrArrayBase ( sruint sizeofx, const SrArrayBase& a );
|
||||
|
||||
/*! Constructor from a given buffer. No checkings are done,
|
||||
its the user responsability to give consistent parameters. */
|
||||
SrArrayBase ( void* pt, int s, int c );
|
||||
|
||||
/*! Will free the internal buffer if needed. The internal size and capacity
|
||||
are not adjusted, so that this method should be called only by the
|
||||
destructor of the derived class, as SrArrayBase has not a destructor. */
|
||||
void free_data ();
|
||||
|
||||
/*! Changes the size of the array. Reallocation is done only when the size
|
||||
requested is greater than the current capacity, and in this case,
|
||||
capacity becomes equal to the size. */
|
||||
void size ( unsigned sizeofx, int ns );
|
||||
|
||||
/*! Changes the capacity of the array. Reallocation is done whenever a new
|
||||
capacity is requested. Internal memory is freed in case of 0 capacity.
|
||||
The size is always kept inside [0,nc]. Parameter nc is considered 0
|
||||
if it is negative. */
|
||||
void capacity ( unsigned sizeofx, int nc );
|
||||
|
||||
/*! Returns a valid index as if the given index references a circular
|
||||
array, ie, it returns index%size() for positive numbers. Negative
|
||||
numbers are also correctly mapped. */
|
||||
int validate ( int index ) const;
|
||||
|
||||
/*! Makes size==capacity, freeing all extra capacity if any. */
|
||||
void compress ( sruint sizeofx );
|
||||
|
||||
/*! Removes positions starting with pos i, and length n, moving all data
|
||||
correctly. The parameters must be valid, no checkings are done. No
|
||||
reallocation is done. */
|
||||
void remove ( sruint sizeofx, int i, int n );
|
||||
|
||||
/*! Inserts dp positions, starting at pos i, moving all data correctly.
|
||||
Parameter i can be between 0 and size(), if i==size(), n positions
|
||||
are appended. If reallocation is needed, the array capacity is
|
||||
reallocated to contain two times the new size (after insertion). */
|
||||
void insert ( sruint sizeofx, int i, int n );
|
||||
|
||||
/*! Copies n entries from src position to dest position. Regions are
|
||||
allowed to overlap. Uses the C function memmove. */
|
||||
void move ( sruint sizeofx, int dest, int src, int n );
|
||||
|
||||
/*! Sorts the array, with the compare function cmp, by calling the
|
||||
system function qsort(). */
|
||||
void sort ( sruint sizeofx, srcompare cmp );
|
||||
|
||||
/*! Linear search, returns index of the element found, or -1 if not found */
|
||||
int lsearch ( sruint sizeofx, const void *x, srcompare cmp ) const;
|
||||
|
||||
/*! Binary search for sorted arrays. Returns index of the element found,
|
||||
or -1 if not found. If not found and pos is not null, pos will have
|
||||
the position to insert the element keeping the array sorted. Faster
|
||||
than the standard C library function bsearch() for large arrays. */
|
||||
int bsearch ( sruint sizeofx, const void *x, srcompare cmp, int *pos ) const;
|
||||
|
||||
/*! Returns the position of the insertion, or -1 if not inserted. The
|
||||
space of sizeofx is created and the index of the position is returned,
|
||||
but get attention to the fact that the contents of x are not moved to
|
||||
the inserted position. In this way, x can be only a key to the data
|
||||
stored in the array. In case of duplication, the insertion is not done
|
||||
if parameter allowdup is given as false. To insert the element position,
|
||||
insert() method is called. */
|
||||
int insort ( sruint sizeofx, const void *x, srcompare cmp, bool allowdup );
|
||||
|
||||
/*! Copy from another array. SrArrayBase will be an exact copy of the given array,
|
||||
allocating the same capacity, but copying only size elements. */
|
||||
void copy ( sruint sizeofx, const SrArrayBase& a );
|
||||
|
||||
/*! Returns the internal buffer pointer that will be null or contain the address of
|
||||
the memory used and that was allocated with malloc(). The user will then be
|
||||
responsible to free this allocated memory with free(). After this call, the
|
||||
array becomes an empty valid array. */
|
||||
void* leave_data ();
|
||||
|
||||
/*! Takes the data of the given array a, that will become an empty array.
|
||||
SrArrayBase will have the same data that a had before. This is done
|
||||
without reallocation. */
|
||||
void take_data ( SrArrayBase& a );
|
||||
|
||||
/*! Frees the current data of SrArrayBase, and then makes SrArrayBase to control
|
||||
the given buffer pt, with size and capacity as given. */
|
||||
void take_data ( void* pt, int s, int c );
|
||||
};
|
||||
|
||||
/*! \class SrArray sr_array.h
|
||||
\brief Fast resizeable dynamic array
|
||||
|
||||
All memory management functions of SrArray use quick memory block functions
|
||||
and so be aware that constructors and destructors of class X are not called.
|
||||
SrArray can be used only with classes or structs that do not have any internal
|
||||
allocated data, as SrArray will not respect them when resizing. Internally,
|
||||
malloc(), realloc() and free() functions are used throught SrArrayBase methods.
|
||||
Note that the array size is automatically reallocated when needed (with a double
|
||||
size strategey), and so take care to not reference internal memory of SrArray
|
||||
that can be reallocated. For example, the following code is wrong: a.push()=a[x],
|
||||
because a[x] referentiates a memory space that can be reallocated by push() */
|
||||
template <class X>
|
||||
class SrArray : protected SrArrayBase
|
||||
{ public:
|
||||
|
||||
/*! Constructs with the given size and capacity. If the given capacity
|
||||
is smaller than the size, capacity is set to be equal to size. */
|
||||
SrArray ( int s=0, int c=0 ) : SrArrayBase ( sizeof(X), s, c ) {}
|
||||
|
||||
/*! For compatibility with prior versions we provide a constructor from
|
||||
3 ints, the last one is simply not considered. */
|
||||
SrArray ( int s, int c, int g ) : SrArrayBase ( sizeof(X), s, c ) {}
|
||||
|
||||
/*! Copy constructor. SrArray will be an exact copy of the given array,
|
||||
but allocating as capacity only the size of a.
|
||||
Attention: the operator= that X might have is not called ! */
|
||||
SrArray ( const SrArray& a ) : SrArrayBase ( sizeof(X), a ) {}
|
||||
|
||||
/*! Constructor from a given buffer. No checkings are done, its the user
|
||||
responsability to give consistent parameters. */
|
||||
SrArray ( X* pt, int s, int c ) : SrArrayBase ( (void*)pt, s, c ) {}
|
||||
|
||||
/*! Destructor frees the array calling the base class free_data() method.
|
||||
Attention: elements' destructors are not called ! */
|
||||
~SrArray () { SrArrayBase::free_data(); }
|
||||
|
||||
/*! Returns true if the array has no elements, ie, size()==0; and false otherwise. */
|
||||
bool empty () const { return _size==0? true:false; }
|
||||
|
||||
/*! Returns the capacity of the array. Capacity is used to be able to have a
|
||||
larger storage buffer than the current size used. The method capacity()
|
||||
will always return a value not smaller than size(). */
|
||||
int capacity () const { return _capacity; }
|
||||
|
||||
/*! Returns the current size of the array. */
|
||||
int size () const { return _size; }
|
||||
|
||||
/*! Changes the size of the array. Reallocation is done only when the size
|
||||
requested is greater than the current capacity, and in this case, capacity
|
||||
becomes equal to the size. */
|
||||
void size ( int ns ) { SrArrayBase::size(sizeof(X),ns); }
|
||||
|
||||
/*! Changes the capacity of the array. Reallocation is done whenever a new
|
||||
capacity is requested. Internal memory is freed in case of 0 capacity.
|
||||
The size is always kept inside [0,nc]. Parameter nc is considered 0
|
||||
if it is negative. */
|
||||
void capacity ( int nc ) { SrArrayBase::capacity(sizeof(X),nc); }
|
||||
|
||||
/*! Defines a minimum capacity to use, ie, sets the capacity to be c
|
||||
iff the current capacity is lower than c */
|
||||
void ensure_capacity ( int c ) { if ( capacity()<c ) capacity(c); }
|
||||
|
||||
/*! Sets all elements as x, copying each element using operator = */
|
||||
void setall ( const X& x )
|
||||
{ int i; for ( i=0; i<_size; i++ ) ((X*)_data)[i]=x; }
|
||||
|
||||
/*! Makes capacity to be equal to size, freeing all extra capacity if any. */
|
||||
void compress () { SrArrayBase::compress ( sizeof(X) ); }
|
||||
|
||||
/*! Returns a valid index as if the given index references a circular
|
||||
array, ie, it returns index%size() for positive numbers. Negative
|
||||
numbers are also correctly mapped. */
|
||||
int validate ( int index ) const { return SrArrayBase::validate(index); }
|
||||
|
||||
/*! Gets a const reference to the element of index i. Indices start from 0 and must
|
||||
be smaller than size(). No checkings are done to ensure that i is valid. */
|
||||
const X& const_get ( int i ) const { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Gets a reference to the element of index i. Indices start from 0 and must
|
||||
be smaller than size(). No checkings are done to ensure that i is valid. */
|
||||
X& get ( int i ) const { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Sets an element. Operator = is used here. Indices start from 0 and must
|
||||
be smaller than size(). No checkings are done to ensure that i is valid. */
|
||||
void set ( int i, const X& x ) { ((X*)_data)[i]=x; }
|
||||
|
||||
/*! Operator version of X& get(int i), but returning a non const reference.
|
||||
No checkings are done to ensure that i is valid. */
|
||||
X& operator[] ( int i ) { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Returns a const pointer of the internal buffer. The internal buffer
|
||||
will always contain a contigous storage space of capacity() elements.
|
||||
See also take_data() and leave_data() methods. */
|
||||
operator const X* () const { return (X*)_data; }
|
||||
|
||||
/*! Returns a reference to the last element, ie, with index size()-1.
|
||||
The array must not be empty when calling this method. */
|
||||
X& top () { return ((X*)_data)[_size-1]; }
|
||||
|
||||
/*! Returns a reference to the last element, ie, with index size()-1, and
|
||||
then reduces the size of the array by one with no reallocation.
|
||||
The array must not be empty when calling this method. */
|
||||
X& pop () { return ((X*)_data)[--_size]; }
|
||||
|
||||
/*! Method to append positions. If reallocation is needed, capacity is set
|
||||
to two times the new size. The first new element appended is returned
|
||||
as a reference. */
|
||||
X& push () { SrArrayBase::insert(sizeof(X),_size,1); return top(); }
|
||||
|
||||
/*! Pushes one position at the end of the array using the insert() method, and
|
||||
then copies the content of x using operator=(). */
|
||||
void push ( const X& x ) { SrArrayBase::insert(sizeof(X),_size,1); top()=x; }
|
||||
|
||||
/*! Inserts dp positions, starting at pos i, moving all data correctly.
|
||||
Parameter i can be between 0 and size(), if i==size(), dp positions are
|
||||
appended. If reallocation is required, capacity is set to two times the
|
||||
new size. The first new element inserted (i) is returned as a reference.
|
||||
The quantity of appended positions (dp) has a default value of 1. */
|
||||
X& insert ( int i, int dp=1 ) { SrArrayBase::insert(sizeof(X),i,dp); return ((X*)_data)[i]; }
|
||||
|
||||
/*! Removes dp positions starting from pos i, moving all data correctly;
|
||||
dp has a default value of 1. Attention: elements' destructors are not called! */
|
||||
void remove ( int i, int dp=1 ) { SrArrayBase::remove(sizeof(X),i,dp); }
|
||||
|
||||
/*! Copies n entries from src position to dest position. Regions are
|
||||
allowed to overlap. Uses the C function memmove. */
|
||||
void move ( int dest, int src, int n ) { SrArrayBase::move(sizeof(X),dest,src,n); }
|
||||
|
||||
/*! Copies all internal data of a to SrArray, with fast memcpy() functions,
|
||||
so that the operator=() that X might have is not used. This method has
|
||||
no effect if a "self copy" is called. */
|
||||
void operator = ( const SrArray<X>& a )
|
||||
{ SrArrayBase::copy ( sizeof(X), a ); }
|
||||
|
||||
/*! Revert the order of the elements in the array. Copy operator of X is used. */
|
||||
void revert ()
|
||||
{ int i, max=size()-1, mid=size()/2; X tmp;
|
||||
for ( i=0; i<mid; i++ ) { SR_SWAP(get(i),get(max-i)); }
|
||||
}
|
||||
|
||||
/*! Inserts the element x in the sorted array, moving all data correctly, and
|
||||
returning the position of the element inserted. When allowdup is false,
|
||||
the element will not be inserted in case of duplication, and in this case,
|
||||
-1 is returned. A compare function int sr_compare(const X*,const X*) is
|
||||
required as argument. The method insert() is called to open the required
|
||||
space, but the operator=() of X will be used to copy element contents in
|
||||
the open position. Parameter allowdup has a default value of true. */
|
||||
int insort ( const X& x, SR_COMPARE_FUNC, bool allowdup=true )
|
||||
{ int pos = SrArrayBase::insort ( sizeof(X), (void*)&x, (srcompare)sr_compare_func, allowdup );
|
||||
if ( pos>=0 ) ((X*)_data)[pos]=x;
|
||||
return pos;
|
||||
}
|
||||
|
||||
/*! Standard library qsort() wrapper call. The compare function is required
|
||||
as argument: int sr_compare(const X*,const X*) */
|
||||
void sort ( SR_COMPARE_FUNC ) { SrArrayBase::sort ( sizeof(X), (srcompare)sr_compare_func ); }
|
||||
|
||||
/*! Linear search, returns the index of the element found, or -1 if not
|
||||
found. A compare function is required as argument:
|
||||
int sr_compare(const X*,const X*) */
|
||||
int lsearch ( const X& x, SR_COMPARE_FUNC ) const { return SrArrayBase::lsearch ( sizeof(X), (void*)&x, (srcompare)sr_compare_func ); }
|
||||
|
||||
/*! Binary search for sorted arrays. Returns index of the element found,
|
||||
or -1 if not found. If not found and pos is not null, pos will have the
|
||||
position to insert the element keeping the array sorted. Faster than
|
||||
the standard C library bsearch() for large arrays. A compare function
|
||||
is required as argument: int sr_compare(const X*,const X*) */
|
||||
int bsearch ( const X& x, SR_COMPARE_FUNC, int *pos=NULL ) const
|
||||
{ return SrArrayBase::bsearch ( sizeof(X), (void*)&x, (srcompare)sr_compare_func, pos ); }
|
||||
|
||||
/*! Returns the internal buffer pointer that will be null or contain the address of
|
||||
the memory used and that was allocated with malloc(). The user will then be
|
||||
responsible to free this allocated memory with free(). After this call, the
|
||||
array becomes an empty valid array. */
|
||||
X* leave_data () { return (X*) SrArrayBase::leave_data(); }
|
||||
|
||||
/*! Frees the data of SrArray, and then makes SrArray be the given array a.
|
||||
After this, a is set to be a valid empty array. The data is moved without
|
||||
reallocation. */
|
||||
void take_data ( SrArray<X>& a ) { SrArrayBase::take_data ( (SrArrayBase&)a ); }
|
||||
|
||||
/*! Frees the data of SrArray, and then makes SrArray to control the given
|
||||
buffer pt, with size and capacity as given. Its the user reponsibility to
|
||||
pass correct values. Note also that the memory menagement of SrArray is
|
||||
done with malloc/realloc/free functions. */
|
||||
void take_data ( X* pt, int s, int c ) { SrArrayBase::take_data ( pt, s, c ); }
|
||||
|
||||
/*! Output all elements of the array. Element type X must have its ouput operator <<
|
||||
available. The output format is [e0 e1 ... en]. */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrArray<X>& a )
|
||||
{ int i;
|
||||
o << '[';
|
||||
for ( i=0; i<a.size(); i++ )
|
||||
{ o << a[i];
|
||||
if ( i<a.size()-1 ) o<<srspc;
|
||||
}
|
||||
return o << ']';
|
||||
}
|
||||
|
||||
/*! Input all elements of the array. Element type X must have its input operator <<
|
||||
available. */
|
||||
friend SrInput& operator>> ( SrInput& in, SrArray<X>& a )
|
||||
{ a.size(0);
|
||||
in.get_token();
|
||||
while (true)
|
||||
{ in.get_token();
|
||||
if ( in.last_token()[0]==']' ) break;
|
||||
in.unget_token();
|
||||
a.push();
|
||||
in >> a.top();
|
||||
}
|
||||
return in;
|
||||
}
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
#endif // SR_ARRAY_H
|
||||
|
||||
202
source/dcdt/se/sr_array_pt.cpp
Normal file
202
source/dcdt/se/sr_array_pt.cpp
Normal file
@ -0,0 +1,202 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_array_pt.h"
|
||||
|
||||
//====================== SrArrayPtBase ==========================
|
||||
|
||||
SrArrayPtBase::SrArrayPtBase ( SrClassManagerBase* m )
|
||||
{
|
||||
_man = m;
|
||||
_man->ref();
|
||||
}
|
||||
|
||||
SrArrayPtBase::SrArrayPtBase ( const SrArrayPtBase& a )
|
||||
{
|
||||
_man = a._man;
|
||||
_man->ref();
|
||||
*this = a; // calls copy operator
|
||||
}
|
||||
|
||||
SrArrayPtBase::~SrArrayPtBase ()
|
||||
{
|
||||
init ();
|
||||
_man->unref(); // must be called after init()!
|
||||
}
|
||||
|
||||
void SrArrayPtBase::init ()
|
||||
{
|
||||
while ( SrArray<void*>::size()>0 ) _man->free ( SrArray<void*>::pop() );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::size ( int ns )
|
||||
{
|
||||
int i, s = SrArray<void*>::size();
|
||||
if ( ns>s )
|
||||
{ SrArray<void*>::size(ns);
|
||||
for ( i=s; i<ns; i++ ) SrArray<void*>::set ( i, _man->alloc() );
|
||||
}
|
||||
else if ( ns<s )
|
||||
{ for ( i=ns; i<s; i++ ) _man->free ( SrArray<void*>::get(i) );
|
||||
SrArray<void*>::size(ns);
|
||||
}
|
||||
}
|
||||
|
||||
void SrArrayPtBase::capacity ( int nc )
|
||||
{
|
||||
int i, s = SrArray<void*>::size();
|
||||
if ( nc<0 ) nc=0;
|
||||
if ( nc<s )
|
||||
{ for ( i=nc; i<s; i++ ) _man->free ( SrArray<void*>::get(i) );
|
||||
}
|
||||
SrArray<void*>::capacity(nc);
|
||||
}
|
||||
|
||||
void SrArrayPtBase::compress ()
|
||||
{
|
||||
capacity ( size() );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::swap ( int i, int j )
|
||||
{
|
||||
void *pti = SrArray<void*>::get(i);
|
||||
void *ptj = SrArray<void*>::get(j);
|
||||
SrArray<void*>::set ( i, ptj );
|
||||
SrArray<void*>::set ( j, pti );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::set ( int i, const void* pt )
|
||||
{
|
||||
SR_ASSERT ( i>=0 && i<size() );
|
||||
_man->free ( SrArray<void*>::get(i) );
|
||||
SrArray<void*>::set ( i, _man->alloc(pt) );
|
||||
}
|
||||
|
||||
void* SrArrayPtBase::get ( int i ) const
|
||||
{
|
||||
SR_ASSERT ( i>=0 && i<size() );
|
||||
return SrArray<void*>::get(i);
|
||||
}
|
||||
|
||||
const void* SrArrayPtBase::const_get ( int i ) const
|
||||
{
|
||||
SR_ASSERT ( i>=0 && i<size() );
|
||||
return SrArray<void*>::const_get(i);
|
||||
}
|
||||
|
||||
void* SrArrayPtBase::top () const
|
||||
{
|
||||
if ( size()==0 ) return 0;
|
||||
return SrArray<void*>::get ( size()-1 );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::pop ()
|
||||
{
|
||||
if ( size()>0 ) _man->free ( SrArray<void*>::pop() );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::push ()
|
||||
{
|
||||
SrArray<void*>::push() = _man->alloc ();
|
||||
}
|
||||
|
||||
void SrArrayPtBase::insert ( int i, int dp )
|
||||
{
|
||||
SrArray<void*>::insert ( i, dp );
|
||||
int j;
|
||||
for ( j=0; j<dp; j++ )
|
||||
SrArray<void*>::set ( i+j, _man->alloc() );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::remove ( int i, int dp )
|
||||
{
|
||||
int j;
|
||||
for ( j=0; j<dp; j++ )
|
||||
_man->free( SrArray<void*>::get(i+j) );
|
||||
SrArray<void*>::remove ( i, dp );
|
||||
}
|
||||
|
||||
void* SrArrayPtBase::extract ( int i )
|
||||
{
|
||||
void *pt = SrArray<void*>::get(i);
|
||||
SrArray<void*>::remove ( i );
|
||||
return pt;
|
||||
}
|
||||
|
||||
void SrArrayPtBase::operator = ( const SrArrayPtBase& a )
|
||||
{
|
||||
init (); // deletes all data
|
||||
SrArray<void*>::size ( a.size() );
|
||||
SrArray<void*>::compress();
|
||||
int i;
|
||||
for ( i=0; i<a.size(); i++ ) SrArray<void*>::set ( i, _man->alloc(a[i]) );
|
||||
}
|
||||
|
||||
static SrClassManagerBase* StaticManager = 0; // This is not thread safe...
|
||||
|
||||
static int fcmp ( const void* pt1, const void* pt2 )
|
||||
{
|
||||
typedef const int* cint;
|
||||
return StaticManager->compare( (const void*)*cint(pt1), (const void*)*cint(pt2) );
|
||||
}
|
||||
|
||||
int SrArrayPtBase::insort ( const void* pt, bool allowdup )
|
||||
{
|
||||
int pos;
|
||||
pos = SrArray<void*>::insort ( (void *const&)pt,
|
||||
(int(*)(void *const *,void *const *))fcmp,
|
||||
allowdup );
|
||||
if ( pos>=0 ) SrArray<void*>::get(pos) = _man->alloc(pt);
|
||||
return pos;
|
||||
}
|
||||
|
||||
void SrArrayPtBase::sort ()
|
||||
{
|
||||
StaticManager = _man;
|
||||
SrArray<void*>::sort ( (int(*)(void *const *,void *const *))fcmp );
|
||||
}
|
||||
|
||||
int SrArrayPtBase::lsearch ( const void* pt ) const
|
||||
{
|
||||
StaticManager = _man;
|
||||
return SrArray<void*>::lsearch ( (void *const&)pt, (int(*)(void *const *,void *const *))fcmp );
|
||||
}
|
||||
|
||||
int SrArrayPtBase::bsearch ( const void* pt, int *pos )
|
||||
{
|
||||
StaticManager = _man;
|
||||
return SrArray<void*>::bsearch ( (void *const&)pt, (int(*)(void *const *,void *const *))fcmp, pos );
|
||||
}
|
||||
|
||||
void SrArrayPtBase::take_data ( SrArrayPtBase& a )
|
||||
{
|
||||
size(0);
|
||||
capacity(0);
|
||||
SrArray<void*>::take_data(a);
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrArrayPtBase& a )
|
||||
{
|
||||
int i, m;
|
||||
m = a.size()-1;
|
||||
o << '[';
|
||||
for ( i=0; i<=m; i++ )
|
||||
{ a._man->output ( o, a[i] );
|
||||
if ( i<m ) o<<srspc;
|
||||
}
|
||||
return o << ']';
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SrArrayPtBase& a )
|
||||
{
|
||||
a.size(0);
|
||||
in.get_token();
|
||||
while (true)
|
||||
{ in.get_token();
|
||||
if ( in.last_token()[0]==']' ) break;
|
||||
in.unget_token ();
|
||||
a.push ();
|
||||
a._man->input ( in, a.top() );
|
||||
}
|
||||
return in;
|
||||
}
|
||||
|
||||
//=========================== EOF ===============================
|
||||
169
source/dcdt/se/sr_array_pt.h
Normal file
169
source/dcdt/se/sr_array_pt.h
Normal file
@ -0,0 +1,169 @@
|
||||
|
||||
# ifndef SR_ARRAY_PT_H
|
||||
# define SR_ARRAY_PT_H
|
||||
|
||||
/** \file sr_array_pt.h
|
||||
* resizeable array of class pointers */
|
||||
|
||||
# include "sr_class_manager.h"
|
||||
# include "sr_array.h"
|
||||
|
||||
/*! \class SrArrayPtBase sr_array_pt.h
|
||||
\brief resizeable array of class pointers
|
||||
|
||||
SrArrayPtBase implements methods for managing a resizeable array
|
||||
of pointers. The user should however use the template
|
||||
class SrArrayPt for an implementation that includes automatic
|
||||
type casts for the user types. A manager to the user
|
||||
data is required, see sr_class_manager.h */
|
||||
class SrArrayPtBase : private SrArray<void*>
|
||||
{ private :
|
||||
SrClassManagerBase* _man;
|
||||
|
||||
public :
|
||||
|
||||
/*! Initiates an empty array. The class manager is required. */
|
||||
SrArrayPtBase ( SrClassManagerBase* m );
|
||||
|
||||
/*! Copy constructor. The class manager of a is shared. */
|
||||
SrArrayPtBase ( const SrArrayPtBase& a );
|
||||
|
||||
/*! Destructor */
|
||||
~SrArrayPtBase ();
|
||||
|
||||
/*! Returns true if the array has no elements, and false otherwise. */
|
||||
bool empty () const { return SrArray<void*>::empty(); }
|
||||
|
||||
/*! Returns the capacity of the array. */
|
||||
int capacity () const { return SrArray<void*>::capacity(); }
|
||||
|
||||
/*! Returns the current size of the array. */
|
||||
int size () const { return SrArray<void*>::size(); }
|
||||
|
||||
/*! Makes the array empty; equivalent to size(0) */
|
||||
void init ();
|
||||
|
||||
/*! Changes the size of the array, filling new objects in the new positions. */
|
||||
void size ( int ns );
|
||||
|
||||
/*! Changes the capacity of the array. */
|
||||
void capacity ( int nc );
|
||||
|
||||
/*! Makes capacity to be equal to size. */
|
||||
void compress ();
|
||||
|
||||
/*! Swaps the pointers of position i and j, that must be valid positions. */
|
||||
void swap ( int i, int j );
|
||||
|
||||
/*! Returns a valid index as if the given index references a circular
|
||||
array, ie, it returns index%size() for positive numbers. Negative
|
||||
numbers are also correctly mapped. */
|
||||
int validate ( int index ) const { SrArray<void*>::validate(index); }
|
||||
|
||||
/*! deletes element i and reallocates a new one as a copy of pt */
|
||||
void set ( int i, const void* pt );
|
||||
|
||||
/*! returns a pointer to the object in position i. */
|
||||
void* get ( int i ) const;
|
||||
|
||||
/*! Returns a const pointer to the object in position i. */
|
||||
const void* const_get ( int i ) const;
|
||||
|
||||
/*! Returns a pointer to the last element or 0 if the array is empty*/
|
||||
void* top () const;
|
||||
|
||||
/*! Pop and frees element size-1 if the array is not empty */
|
||||
void pop ();
|
||||
|
||||
/*! Allocates and appends one empty element */
|
||||
void push ();
|
||||
|
||||
/*! Inserts n positions, starting at pos i, and putting a new element in
|
||||
each new position created. */
|
||||
void insert ( int i, int n=1 );
|
||||
|
||||
/*! Removes n positions starting from pos i */
|
||||
void remove ( int i, int n=1 );
|
||||
|
||||
/*! Extract (without deletion) and returns the pointer at position i */
|
||||
void* extract ( int i );
|
||||
|
||||
/*! Copy operator */
|
||||
void operator = ( const SrArrayPtBase& a );
|
||||
|
||||
/*! Inserts one object, considering the array is sorted. Returns the
|
||||
inserted position, or -1 if duplication occurs and allowdup is false.
|
||||
(Note: all methods using sr_compare functions are not thread safe, as
|
||||
they use a static pointer to set the current comparison function) */
|
||||
int insort ( const void* pt, bool allowdup=true );
|
||||
|
||||
/*! Sort array */
|
||||
void sort ();
|
||||
|
||||
/*! Linear search. Returns index of the element found,
|
||||
or -1 if not found. */
|
||||
int lsearch ( const void* pt ) const;
|
||||
|
||||
/*! Binary search for sorted arrays. Returns index of the element found,
|
||||
or -1 if not found. If not found and pos is not 0, pos will have the
|
||||
position to insert the element keeping the array sorted. */
|
||||
int bsearch ( const void* pt, int *pos=NULL );
|
||||
|
||||
/*! Frees all data, and then makes SrArrayPtBase be the given array a.
|
||||
After this, a is set to be a valid empty array. The data is moved without
|
||||
reallocation. */
|
||||
void take_data ( SrArrayPtBase& a );
|
||||
|
||||
/*! Outputs all elements of the array in format is [e0 e1 ... en]. */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrArrayPtBase& a );
|
||||
|
||||
/*! Inputs elements in format is [e0 e1 ... en]. */
|
||||
friend SrInput& operator>> ( SrInput& in, SrArrayPtBase& a );
|
||||
};
|
||||
|
||||
/*! \class SrArrayPt sr_array_pt.h
|
||||
\brief resizeable array of class pointers
|
||||
|
||||
SrArrayPtBase implements methods for managing a resizeable array
|
||||
of pointers, which are managed by SrClassManager object. */
|
||||
template <class X>
|
||||
class SrArrayPt : public SrArrayPtBase
|
||||
{ public :
|
||||
/*! Default constructor that automatically creates a SrClassManager<X>. */
|
||||
SrArrayPt () : SrArrayPtBase ( new SrClassManager<X> ) {}
|
||||
|
||||
/*! Constructor with a given class manager. */
|
||||
SrArrayPt ( SrClassManagerBase* m ) : SrArrayPtBase ( m ) {}
|
||||
|
||||
/*! Copy constructor sharing class manager. */
|
||||
SrArrayPt ( const SrArrayPt& a ) : SrArrayPtBase ( a ) {}
|
||||
|
||||
void set ( int i, const X& x ) { SrArrayPtBase::set(i,(const void*)&x); }
|
||||
|
||||
X* get ( int i ) { return (X*)SrArrayPtBase::get(i); }
|
||||
const X* const_get ( int i ) const { return (const X*)SrArrayPtBase::const_get(i); }
|
||||
|
||||
X* operator[] ( int i ) { return get(i); }
|
||||
|
||||
/*! Returns a pointer to the last element or 0 if the array is empty*/
|
||||
X* top () const { return (X*)SrArrayPtBase::top(); }
|
||||
|
||||
/*! Pop and frees element size-1 if the array is not empty */
|
||||
void pop () { SrArrayPtBase::pop(); }
|
||||
|
||||
/*! Allocates and appends one empty element */
|
||||
void push () { SrArrayPtBase::push(); }
|
||||
|
||||
/*! Allocates and appends one element using copy operator */
|
||||
void push ( const X& x ) { push(); *top()=x; }
|
||||
|
||||
/*! Allocates and insert one element using copy operator */
|
||||
void insert ( int i, const X& x ) { insert(i,1); *get(i)=x; }
|
||||
|
||||
/*! Extract (without deletion) and returns the pointer at position i */
|
||||
X* extract ( int i ) { return (X*) SrArrayPtBase::extract(i); }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
#endif // SR_ARRAY_PT_H
|
||||
193
source/dcdt/se/sr_box.cpp
Normal file
193
source/dcdt/se/sr_box.cpp
Normal file
@ -0,0 +1,193 @@
|
||||
#include "precompiled.h"
|
||||
|
||||
# include "sr_box.h"
|
||||
# include "sr_mat.h"
|
||||
|
||||
//======================================== SrBox =======================================
|
||||
|
||||
const char* SrBox::class_name = "Box";
|
||||
|
||||
SrBox::SrBox ( const SrBox& x, const SrBox& y )
|
||||
: a ( SR_MIN(x.a.x,y.a.x), SR_MIN(x.a.y,y.a.y), SR_MIN(x.a.z,y.a.z) ),
|
||||
b ( SR_MAX(x.b.x,y.b.x), SR_MAX(x.b.y,y.b.y), SR_MAX(x.b.z,y.b.z) )
|
||||
{
|
||||
}
|
||||
|
||||
bool SrBox::empty () const
|
||||
{
|
||||
return a.x>b.x || a.y>b.y || a.z>b.z ? true:false;
|
||||
}
|
||||
|
||||
float SrBox::volume () const
|
||||
{
|
||||
return (b.x-a.x) * (b.y-a.y) * (b.z-a.z);
|
||||
}
|
||||
|
||||
SrVec SrBox::center () const
|
||||
{
|
||||
return (a+b)/2.0f; // == a + (b-a)/2 == a + b/2 - a/2
|
||||
}
|
||||
|
||||
void SrBox::center ( const SrVec& p )
|
||||
{
|
||||
(*this) += p-center();
|
||||
}
|
||||
|
||||
void SrBox::size ( const SrVec& v )
|
||||
{
|
||||
b = a+v;
|
||||
}
|
||||
|
||||
SrVec SrBox::size () const
|
||||
{
|
||||
return b-a;
|
||||
}
|
||||
|
||||
float SrBox::max_size () const
|
||||
{
|
||||
SrVec s = b-a;
|
||||
return SR_MAX3(s.x,s.y,s.z);
|
||||
}
|
||||
|
||||
float SrBox::min_size () const
|
||||
{
|
||||
SrVec s = b-a;
|
||||
return SR_MIN3(s.x,s.y,s.z);
|
||||
}
|
||||
|
||||
void SrBox::extend ( const SrPnt &p )
|
||||
{
|
||||
if ( empty() ) { a=p; b=p; }
|
||||
SR_UPDMIN ( a.x, p.x ); SR_UPDMAX ( b.x, p.x );
|
||||
SR_UPDMIN ( a.y, p.y ); SR_UPDMAX ( b.y, p.y );
|
||||
SR_UPDMIN ( a.z, p.z ); SR_UPDMAX ( b.z, p.z );
|
||||
}
|
||||
|
||||
void SrBox::extend ( const SrBox &box )
|
||||
{
|
||||
if ( empty() ) *this=box;
|
||||
if ( box.empty() ) return;
|
||||
SR_UPDMIN ( a.x, box.a.x ); SR_UPDMAX ( b.x, box.b.x );
|
||||
SR_UPDMIN ( a.y, box.a.y ); SR_UPDMAX ( b.y, box.b.y );
|
||||
SR_UPDMIN ( a.z, box.a.z ); SR_UPDMAX ( b.z, box.b.z );
|
||||
}
|
||||
|
||||
void SrBox::grows ( float dx, float dy, float dz )
|
||||
{
|
||||
a.x-=dx; a.y-=dy; a.z-=dz;
|
||||
b.x+=dx; b.y+=dy; b.z+=dz;
|
||||
}
|
||||
|
||||
bool SrBox::contains ( const SrVec& p ) const
|
||||
{
|
||||
return p.x<a.x || p.y<a.y || p.z<a.z || p.x>b.x || p.y>b.y || p.z>b.z ? false : true;
|
||||
}
|
||||
|
||||
bool SrBox::intersects ( const SrBox& box ) const
|
||||
{
|
||||
if ( box.contains(a) ) return true;
|
||||
if ( box.contains(b) ) return true;
|
||||
SrVec x(a.x,a.y,b.z); if ( box.contains(x) ) return true;
|
||||
x.set (a.x,b.y,a.z); if ( box.contains(x) ) return true;
|
||||
x.set (b.x,a.y,a.z); if ( box.contains(x) ) return true;
|
||||
x.set (b.x,b.y,a.z); if ( box.contains(x) ) return true;
|
||||
x.set (b.x,a.y,b.z); if ( box.contains(x) ) return true;
|
||||
x.set (a.x,b.y,b.z); if ( box.contains(x) ) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
void SrBox::get_side ( SrPnt& p1, SrPnt& p2, SrPnt& p3, SrPnt& p4, int s ) const
|
||||
{
|
||||
switch (s)
|
||||
{ case 0 : p1.set ( a.x, a.y, a.z );
|
||||
p2.set ( a.x, a.y, b.z );
|
||||
p3.set ( a.x, b.y, b.z );
|
||||
p4.set ( a.x, b.y, a.z );
|
||||
break;
|
||||
case 1 : p1.set ( b.x, a.y, a.z );
|
||||
p2.set ( b.x, b.y, a.z );
|
||||
p3.set ( b.x, b.y, b.z );
|
||||
p4.set ( b.x, a.y, b.z );
|
||||
break;
|
||||
case 2 : p1.set ( a.x, a.y, a.z );
|
||||
p2.set ( b.x, a.y, a.z );
|
||||
p3.set ( b.x, a.y, b.z );
|
||||
p4.set ( a.x, a.y, b.z );
|
||||
break;
|
||||
case 3 : p1.set ( a.x, b.y, a.z );
|
||||
p2.set ( a.x, b.y, b.z );
|
||||
p3.set ( b.x, b.y, b.z );
|
||||
p4.set ( b.x, b.y, a.z );
|
||||
break;
|
||||
case 4 : p1.set ( a.x, a.y, a.z );
|
||||
p2.set ( a.x, b.y, a.z );
|
||||
p3.set ( b.x, b.y, a.z );
|
||||
p4.set ( b.x, a.y, a.z );
|
||||
break;
|
||||
case 5 : p1.set ( a.x, a.y, b.z );
|
||||
p2.set ( b.x, a.y, b.z );
|
||||
p3.set ( b.x, b.y, b.z );
|
||||
p4.set ( a.x, b.y, b.z );
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void SrBox::operator += ( const SrVec& v )
|
||||
{
|
||||
a += v;
|
||||
b += v;
|
||||
}
|
||||
|
||||
void SrBox::operator *= ( float s )
|
||||
{
|
||||
a *= s;
|
||||
b *= s;
|
||||
}
|
||||
|
||||
//============================== friends ========================================
|
||||
|
||||
SrBox operator * ( const SrBox& b, const SrMat& m )
|
||||
{
|
||||
SrBox x; // init as an empty box
|
||||
|
||||
if ( b.empty() ) return x;
|
||||
|
||||
SrVec v(b.a); x.extend(v*m);
|
||||
v.x=b.b.x; x.extend(v*m);
|
||||
v.y=b.b.y; x.extend(v*m);
|
||||
v.x=b.a.x; x.extend(v*m);
|
||||
v.z=b.b.z; x.extend(v*m);
|
||||
v.x=b.b.x; x.extend(v*m);
|
||||
v.y=b.a.y; x.extend(v*m);
|
||||
v.x=b.a.x; x.extend(v*m);
|
||||
return x;
|
||||
}
|
||||
|
||||
SrBox operator * ( const SrMat& m, const SrBox& b )
|
||||
{
|
||||
SrBox x; // init as an empty box
|
||||
|
||||
if ( b.empty() ) return x;
|
||||
|
||||
SrVec v(b.a); x.extend(m*v);
|
||||
v.x=b.b.x; x.extend(m*v);
|
||||
v.y=b.b.y; x.extend(m*v);
|
||||
v.x=b.a.x; x.extend(m*v);
|
||||
v.z=b.b.z; x.extend(m*v);
|
||||
v.x=b.b.x; x.extend(m*v);
|
||||
v.y=b.a.y; x.extend(m*v);
|
||||
v.x=b.a.x; x.extend(m*v);
|
||||
return x;
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrBox& box )
|
||||
{
|
||||
return o << box.a << ' ' << box.b;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SrBox& box )
|
||||
{
|
||||
return in >> box.a >> box.b;
|
||||
}
|
||||
|
||||
//================================ End of File =================================================
|
||||
132
source/dcdt/se/sr_box.h
Normal file
132
source/dcdt/se/sr_box.h
Normal file
@ -0,0 +1,132 @@
|
||||
|
||||
# ifndef SR_BOX_H
|
||||
# define SR_BOX_H
|
||||
|
||||
/** \file sr_box.h
|
||||
* 3d axis-aligned box
|
||||
*/
|
||||
|
||||
# include "sr_vec.h"
|
||||
|
||||
class SrMat;
|
||||
|
||||
/*! \class SrBox sr_box.h
|
||||
\brief 3d axis-aligned box
|
||||
|
||||
SrBox describes a 3d axis-aligned box. The box is described by
|
||||
two 3d vertices a,b; one with the minimum coordinates(a), and the
|
||||
other with the maximum coordinates(b). It is used to describe
|
||||
bounding boxes, which have their sides parallel to the axes.
|
||||
If any of the coordinates of a are greater than any coordinates
|
||||
of b, the box is said to be empty, ie, not valid. */
|
||||
class SrBox
|
||||
{ public :
|
||||
SrPnt a; //!< Contains the minimum coordinates of the box
|
||||
SrPnt b; //!< Contains the maximum coordinates of the box
|
||||
static const char* class_name;
|
||||
public :
|
||||
|
||||
/*! Default constructor initializes the box as the empty box (1,1,1)(0,0,0). */
|
||||
SrBox () : a(SrPnt::one), b(SrPnt::null) {}
|
||||
|
||||
/*! Constructs a box with all vertices the same. This degenerated
|
||||
box is identical to a single point and is not considered an
|
||||
empty box. */
|
||||
SrBox ( const SrPnt& p ) : a(p), b(p) {}
|
||||
|
||||
/*! Constructs the box from the given min and max points. */
|
||||
SrBox ( const SrPnt& min, const SrPnt& max ) : a(min), b(max) {}
|
||||
|
||||
/*! Copy constructor. */
|
||||
SrBox ( const SrBox& box ) : a(box.a), b(box.b) {}
|
||||
|
||||
/* Constructs SrBox containing the two given boxes. */
|
||||
SrBox ( const SrBox& x, const SrBox& y );
|
||||
|
||||
/*! Init the box as (0,0,0)(0,0,0). */
|
||||
void set_null () { a=SrPnt::null; b=SrPnt::null; }
|
||||
|
||||
/*! Sets the minimum and maximum vertices of the box. */
|
||||
void set ( const SrPnt& min, const SrPnt& max ) { a=min; b=max; }
|
||||
|
||||
/*! Sets the box to be empty, ie, invalid, just by putting
|
||||
the x coordinate of the minimum vertex (a) greater than
|
||||
the x coordinate of the maximum vertex (b). */
|
||||
void set_empty () { a.x=1.0; b.x=0.0; }
|
||||
|
||||
/*! Returns true if the box is empty (or invalid), ie, when
|
||||
some coordinate of a is greater than b. */
|
||||
bool empty () const;
|
||||
|
||||
/*! Returns the volume of the box. */
|
||||
float volume () const;
|
||||
|
||||
/*! Returns the center point of the box (b+a)/2. */
|
||||
SrPnt center () const;
|
||||
|
||||
/*! Translates SrBox to have its center in p. */
|
||||
void center ( const SrPnt& p );
|
||||
|
||||
/*! Changes the position of the maximum vertex b of the box in order to
|
||||
achieve the desired dimensions given in v (b=a+v). */
|
||||
void size ( const SrVec& v );
|
||||
|
||||
/*! Returns the dimensions in each axis (b-a). */
|
||||
SrVec size () const;
|
||||
|
||||
/*! Returns the maximum dimension of the box. */
|
||||
float max_size () const;
|
||||
|
||||
/*! Returns the minimum dimension of the box. */
|
||||
float min_size () const;
|
||||
|
||||
/*! Extends SrBox (if needed) to contain the given point. If SrBox
|
||||
is empty, SrBox min and max vertices become the given point. */
|
||||
void extend ( const SrPnt& p );
|
||||
|
||||
/*! Extends SrBox (if needed) to contain the given box, if the given
|
||||
box is not empty(). If SrBox is empty, SrBox becomes the given box. */
|
||||
void extend ( const SrBox& box );
|
||||
|
||||
/*! Adds (dx,dy,dz) to b, and diminish it from a. */
|
||||
void grows ( float dx, float dy, float dz );
|
||||
|
||||
/*! Returns true if SrBox contains the given point. */
|
||||
bool contains ( const SrPnt& p ) const;
|
||||
|
||||
/*! Returns true if SrBox intersects with the given box. */
|
||||
bool intersects ( const SrBox& box ) const;
|
||||
|
||||
/*! Returns the four corners of side s={0,..,5} of the box.
|
||||
Side 0 has all x coordinates equal to a.x, side 1 equal to b.x.
|
||||
Side 2 has all y coordinates equal to a.y, side 3 equal to b.y.
|
||||
Side 4 has all z coordinates equal to a.z, side 5 equal to b.z.
|
||||
Order is ccw, starting with the point with more SrBox::a coordinates */
|
||||
void get_side ( SrPnt& p1, SrPnt& p2, SrPnt& p3, SrPnt& p4, int s ) const;
|
||||
|
||||
/*! The bounding box is identical to SrBox (needed by SrSceneShapeTpl). */
|
||||
void get_bounding_box ( SrBox &box ) const { box=*this; }
|
||||
|
||||
/* Translates SrBox by v. */
|
||||
void operator += ( const SrVec& v );
|
||||
|
||||
/* Scales SrBox by the factor s. */
|
||||
void operator *= ( float s );
|
||||
|
||||
/* Returns the bounding box of the transformed vertices vM of b. */
|
||||
friend SrBox operator * ( const SrBox& b, const SrMat& m );
|
||||
|
||||
/* Returns the bounding box of the transformed vertices Mv of b. */
|
||||
friend SrBox operator * ( const SrMat& m, const SrBox& b );
|
||||
|
||||
/*! Outputs in format: "x y z a b c". */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrBox& box );
|
||||
|
||||
/*! Inputs from format: "x y z a b c". */
|
||||
friend SrInput& operator>> ( SrInput& in, SrBox& box );
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_BOX_H
|
||||
|
||||
63
source/dcdt/se/sr_buffer.cpp
Normal file
63
source/dcdt/se/sr_buffer.cpp
Normal file
@ -0,0 +1,63 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdlib.h>
|
||||
# include <string.h>
|
||||
|
||||
# include "sr_buffer.h"
|
||||
|
||||
//=== SrBuffer =======================================================================
|
||||
|
||||
# define BUF(i) ((char*)buffer)+(sizeofx*(i))
|
||||
|
||||
void* sr_buffer_size ( void* buffer, int sizeofx, int& size, int newsize )
|
||||
{
|
||||
if ( size==newsize ) return buffer;
|
||||
size = newsize;
|
||||
if ( size==0 )
|
||||
{ if (buffer) { free(buffer); buffer=0; } }
|
||||
else
|
||||
{ buffer = realloc ( buffer, (size_t)(sizeofx*size) ); }
|
||||
return buffer;
|
||||
}
|
||||
|
||||
void* sr_buffer_insert ( void* buffer, int sizeofx, int& size, int i, int dp )
|
||||
{
|
||||
size += dp;
|
||||
buffer = realloc ( buffer, (size_t)(sizeofx*size) );
|
||||
if ( i<size-dp )
|
||||
memmove ( BUF(i+dp), BUF(i), sizeofx*(size-dp-i) ); // ok with overlap
|
||||
return buffer;
|
||||
}
|
||||
|
||||
void* sr_buffer_remove ( void* buffer, int sizeofx, int& size, int i, int dp )
|
||||
{
|
||||
if ( i<size-dp ) memmove ( BUF(i), BUF(i+dp), sizeofx*(size-(i+dp)) );
|
||||
return sr_buffer_size ( buffer, sizeofx, size, size-dp );
|
||||
}
|
||||
|
||||
void* sr_buffer_copy ( void* buffer, int sizeofx, int& size, const void* buffertocp, int sizetocp )
|
||||
{
|
||||
if ( buffer==buffertocp ) return buffer;
|
||||
buffer = sr_buffer_size ( buffer, sizeofx, size, sizetocp );
|
||||
if ( buffer ) memcpy ( buffer, buffertocp, sizeofx*size ); // no overlap
|
||||
return buffer;
|
||||
}
|
||||
|
||||
# undef BUF
|
||||
|
||||
//=== End of File =====================================================================
|
||||
|
||||
/* Note:
|
||||
void *memmove( void *dest, const void *src, size_t count );
|
||||
If some regions of the source area and the destination overlap,
|
||||
memmove ensures that the original source bytes in the overlapping
|
||||
region are copied before being overwritten
|
||||
|
||||
void *memcpy( void *dest, const void *src, size_t count );
|
||||
If the source and destination overlap, memcpy function does not
|
||||
ensure that the original source bytes in the overlapping region
|
||||
are copied before being overwritten. Use memmove to handle
|
||||
overlapping regions. */
|
||||
|
||||
|
||||
|
||||
|
||||
170
source/dcdt/se/sr_buffer.h
Normal file
170
source/dcdt/se/sr_buffer.h
Normal file
@ -0,0 +1,170 @@
|
||||
|
||||
# ifndef SR_BUFFER_H
|
||||
# define SR_BUFFER_H
|
||||
|
||||
/** \file sr_buffer.h
|
||||
* fast buffer memory management template */
|
||||
|
||||
# include "sr.h"
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
|
||||
/*! Allocates memory with sizeofx*newsize bytes using the C function realloc.
|
||||
If newsize is zero, buffer is freed and both buffer and size becomes zero.
|
||||
After the function call, size has the value of newsize. No effect if
|
||||
size==newsize. */
|
||||
void* sr_buffer_size ( void* buffer, int sizeofx, int& size, int newsize );
|
||||
|
||||
/*! Inserts dp*sizeofx bytes at i position, moving correctly the buffer contents. */
|
||||
void* sr_buffer_insert ( void* buffer, int sizeofx, int& size, int i, int dp );
|
||||
|
||||
/*! Inserts dp*sizeofx bytes at i position, moving correctly the buffer contents. */
|
||||
void* sr_buffer_remove ( void* buffer, int sizeofx, int& size, int i, int dp );
|
||||
|
||||
/*! Put buffers with same size and copy them. No effect if buffers pointers are equal. */
|
||||
void* sr_buffer_copy ( void* buffer, int sizeofx, int& size, const void* buffertocp, int sizetocp );
|
||||
|
||||
/*! \class SrBuffer sr_buffer.h
|
||||
\brief fast buffer memory management template
|
||||
|
||||
All memory management functions of SrBuffer are written using the four buffer
|
||||
functions available in this header. Be aware that constructors and destructors
|
||||
of class X are not called. SrBuffer can thus be used only with classes or structs
|
||||
that do not have any internal allocated data.
|
||||
Internally, SrBuffer keeps only a pointer to the allocated
|
||||
buffer memory and the buffer size. For a more featured class see SrArray. */
|
||||
template <class X>
|
||||
class SrBuffer
|
||||
{ private:
|
||||
void* _data;
|
||||
int _size;
|
||||
public:
|
||||
|
||||
/*! Default constructor. */
|
||||
SrBuffer () : _data(0), _size(0) {}
|
||||
|
||||
/*! Copy constructor. */
|
||||
SrBuffer ( const SrBuffer& b ) : _data(0), _size(0)
|
||||
{ _data=sr_buffer_copy(_data,sizeof(X),_size,b._data,b._size); }
|
||||
|
||||
/*! Constructor with a given size. */
|
||||
SrBuffer ( int s ) : _data(0), _size(0) { _data=sr_buffer_size(_data,sizeof(X),_size,s); }
|
||||
|
||||
/*! Constructor from a user allocated buffer. See also leave_data(). */
|
||||
SrBuffer ( X* pt, int s ) : _data(pt), _size(s) {}
|
||||
|
||||
/*! Destructor frees the buffer. Elements' destructors are not called ! */
|
||||
~SrBuffer () { sr_buffer_size ( _data, sizeof(X), _size, 0 ); }
|
||||
|
||||
/*! Returns true if size()==0; and false otherwise. */
|
||||
bool empty () const { return _size==0? true:false; }
|
||||
|
||||
/*! Returns the current size of the Buffer. */
|
||||
int size () const { return _size; }
|
||||
|
||||
/*! Allows to change the size of the buffer. */
|
||||
void size ( int ns )
|
||||
{ _data=sr_buffer_size(_data,sizeof(X),_size,ns); }
|
||||
|
||||
/*! Sets all elements as x, copying each element using operator = */
|
||||
void setall ( const X& x )
|
||||
{ for ( int i=0; i<_size; i++ ) ((X*)_data)[i]=x; }
|
||||
|
||||
/*! Get a const reference to the element of index i. Indices start from 0
|
||||
and must be smaller than size(). No checkings are done to ensure that
|
||||
i is in a valid range. */
|
||||
const X& const_get ( int i ) const { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Get a reference to the element of index i. Indices start from 0
|
||||
and must be smaller than size(). No checkings are done to ensure that
|
||||
i is in a valid range. */
|
||||
X& get ( int i ) { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Sets an element. Operator = is used here. Indices start from 0 and must
|
||||
be smaller than size(). No checkings are done to ensure that i is valid. */
|
||||
void set ( int i, const X& x ) { ((X*)_data)[i]=x; }
|
||||
|
||||
/*! Operator version of X& get(int i), but returning a non const reference.
|
||||
No checkings are done to ensure that i is valid. */
|
||||
X& operator[] ( int i ) { return ((X*)_data)[i]; }
|
||||
|
||||
/*! Returns a const pointer of the internal buffer. The internal buffer
|
||||
will always contain a contigous storage space of size() elements.
|
||||
See also take_data() and leave_data() methods. */
|
||||
operator const X* () const { return (X*)_data; }
|
||||
|
||||
/*! Returns a reference to the last element, ie, with index size()-1.
|
||||
The Buffer must not be empty when calling this method. */
|
||||
X& top () { return ((X*)_data)[_size-1]; }
|
||||
|
||||
/* Reduces the size of the buffer by one. */
|
||||
void pop () { _data=sr_buffer_size(_data,sizeof(X),_size,_size-1); }
|
||||
|
||||
/* Increases the size of the buffer by one. */
|
||||
void push ( int dp=1 ) { _data=sr_buffer_size(_data,sizeof(X),_size,_size+1); }
|
||||
|
||||
/*! Inserts dp positions, starting at pos i, moving all data correctly.
|
||||
Parameter i can be between 0 and size(), if i==size(), dp positions are
|
||||
appended. */
|
||||
void insert ( int i, int dp=1 ) { _data=sr_buffer_insert(_data,sizeof(X),_size,i,dp); }
|
||||
|
||||
/*! Removes dp positions starting from pos i, moving all data correctly;
|
||||
dp has a default value of 1. Attention: elements' destructors are not
|
||||
called ! */
|
||||
void remove ( int i, int dp=1 ) { _data=sr_buffer_remove(_data,sizeof(X),_size,i,dp); }
|
||||
|
||||
/*! Copies all internal data of a to SrBuffer, with fast memcpy() functions,
|
||||
so that the operator=() that X might have is not used. This method has no
|
||||
effect if a "self copy" is called. */
|
||||
void operator = ( const SrBuffer<X>& b )
|
||||
{ _data=sr_buffer_copy(_data,sizeof(X),_size,b._data,b._size); }
|
||||
|
||||
/*! Makes the given xpt pointer to point to the internal buffer, without
|
||||
reallocation; xpt will then be null or contain the address of the memory
|
||||
used and that was allocated with malloc(). The user will then be responsible
|
||||
to free this allocated memory with free(). After this call, SrBuffer will
|
||||
become an empty but valid buffer. */
|
||||
void leave_data ( X*& xpt, int& size )
|
||||
{ xpt=_data; size=_size; _data=0; _size=0; }
|
||||
|
||||
/*! Frees the data of SrBuffer, and then makes SrBuffer be the given buffer b.
|
||||
After this, b is set to be a valid empty buffer. The data is moved without
|
||||
reallocation. */
|
||||
void take_data ( SrBuffer<X>& b ) { size(0); b.leave_data(_data,_size); }
|
||||
|
||||
/*! Outputs all elements of the Buffer. Element type X must have its ouput operator <<
|
||||
available. This is the only method of SrBuffer that is not inline. */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrBuffer<X>& b )
|
||||
{ o << '[';
|
||||
int i;
|
||||
for ( i=0; i<b.size()-1; i++ ) o << b.const_get(i) << srspc;
|
||||
if ( i<b.size() ) o << b.const_get(i);
|
||||
return o << ']';
|
||||
}
|
||||
|
||||
/*! Inputs all elements of the buffer. Element type X must have its input operator <<
|
||||
available. */
|
||||
friend SrInput& operator>> ( SrInput& in, SrBuffer<X>& b )
|
||||
{ int s=0;
|
||||
b.size(128);
|
||||
in.get_token();
|
||||
|
||||
while (true)
|
||||
{ in.get_token();
|
||||
if ( in.last_token()[0]==']' ) break;
|
||||
in.unget_token();
|
||||
in >> b[s];
|
||||
s++;
|
||||
if ( s>=b.size() ) b.size ( b.size()+128 );
|
||||
}
|
||||
|
||||
b.size ( s );
|
||||
|
||||
return in;
|
||||
}
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
#endif // SR_BUFFER_H
|
||||
|
||||
1176
source/dcdt/se/sr_bv.cpp
Normal file
1176
source/dcdt/se/sr_bv.cpp
Normal file
File diff suppressed because it is too large
Load Diff
230
source/dcdt/se/sr_bv_coldet.cpp
Normal file
230
source/dcdt/se/sr_bv_coldet.cpp
Normal file
@ -0,0 +1,230 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_bv_coldet.h"
|
||||
|
||||
//============================= SrBvColdet::Object ===========================
|
||||
|
||||
/*! Contains required data to be stored in each BvTree. */
|
||||
class SrBvColdet::Object
|
||||
{ public :
|
||||
bool active;
|
||||
srbvmat R;
|
||||
srbvvec T;
|
||||
SrBvTree tree;
|
||||
public :
|
||||
Object ()
|
||||
{ SrBvMath::Midentity ( R ); // set to id matrix
|
||||
SrBvMath::Videntity ( T ); // set to a null vector
|
||||
active = true;
|
||||
}
|
||||
|
||||
~Object () {};
|
||||
|
||||
void set_transformation ( const SrMat& srm )
|
||||
{
|
||||
# define SRM(i) (srbvreal)srm.get(i)
|
||||
// here we pass from column-major to line-major order:
|
||||
R[0][0]=SRM(0); R[1][0]=SRM(1); R[2][0]=SRM(2);
|
||||
R[0][1]=SRM(4); R[1][1]=SRM(5); R[2][1]=SRM(6);
|
||||
R[0][2]=SRM(8); R[1][2]=SRM(9); R[2][2]=SRM(10);
|
||||
T[0]=SRM(12); T[1]=SRM(13); T[2]=SRM(14);
|
||||
# undef SRM
|
||||
}
|
||||
|
||||
void make_tree ( const SrModel& model )
|
||||
{ tree.make ( model );
|
||||
}
|
||||
};
|
||||
|
||||
class SrBvColdetObjectManager : public SrClassManagerBase
|
||||
{ public :
|
||||
virtual void* alloc () { return (void*) new SrBvColdet::Object; }
|
||||
virtual void* alloc ( const void* obj ) { return (void*) 0; } // copy not used/supported
|
||||
virtual void free ( void* obj ) { delete (SrBvColdet::Object*) obj; }
|
||||
};
|
||||
|
||||
//================================ SrBvColdet =====================================
|
||||
|
||||
SrBvColdet::SrBvColdet ()
|
||||
{
|
||||
_set = new SrSet<Object> ( new SrBvColdetObjectManager );
|
||||
}
|
||||
|
||||
SrBvColdet::~SrBvColdet ()
|
||||
{
|
||||
delete _set;
|
||||
}
|
||||
|
||||
void SrBvColdet::init ()
|
||||
{
|
||||
_set->init ();
|
||||
_nbody.init ();
|
||||
_disabled.init ();
|
||||
_pairs.size (0);
|
||||
}
|
||||
|
||||
int SrBvColdet::insert_object ( const SrModel& m )
|
||||
{
|
||||
int id = _set->insert ();
|
||||
Object* o = _set->get ( id );
|
||||
o->make_tree ( m );
|
||||
_nbody.insert_object ( id, m );
|
||||
return id;
|
||||
}
|
||||
|
||||
void SrBvColdet::remove_object ( int id )
|
||||
{
|
||||
Object* o = _getobj ( id );
|
||||
if ( !o ) return;
|
||||
_set->remove(id); // will also delete o
|
||||
_disabled.del_pairs_with_id ( id );
|
||||
_nbody.remove_object ( id );
|
||||
}
|
||||
|
||||
void SrBvColdet::update_transformation ( int id, const SrMat& m )
|
||||
{
|
||||
Object* o = _getobj ( id );
|
||||
if ( !o ) return;
|
||||
o->set_transformation ( m );
|
||||
_nbody.update_transformation ( id, m );
|
||||
}
|
||||
|
||||
void SrBvColdet::activate_object ( int id )
|
||||
{
|
||||
Object* o = _getobj ( id );
|
||||
if ( !o ) return;
|
||||
o->active = true;
|
||||
}
|
||||
|
||||
void SrBvColdet::deactivate_object ( int id )
|
||||
{
|
||||
Object* o = _getobj ( id );
|
||||
if ( !o ) return;
|
||||
o->active = false;
|
||||
}
|
||||
|
||||
void SrBvColdet::activate_pair ( int id1, int id2 )
|
||||
{
|
||||
_disabled.del_pair ( id1, id2 );
|
||||
}
|
||||
|
||||
void SrBvColdet::deactivate_pair ( int id1, int id2 )
|
||||
{
|
||||
_disabled.add_pair ( id1, id2 );
|
||||
}
|
||||
|
||||
//============================= private method ================================
|
||||
|
||||
bool SrBvColdet::_collide ( SrBvTreeQuery::CollideFlag flag )
|
||||
{
|
||||
_pairs.size ( 0 );
|
||||
|
||||
const SrArray<SrBvIdPairs::Elem*>& overlap = _nbody.overlapping_pairs.elements();
|
||||
const SrArray<SrBvIdPairs::Elem*>& disabled = _disabled.elements();
|
||||
int osize = overlap.size();
|
||||
int dsize = disabled.size();
|
||||
|
||||
bool call_collide;
|
||||
Object* o1;
|
||||
Object* o2;
|
||||
const SrBvIdPairs::Elem* curr_overlap;
|
||||
const SrBvIdPairs::Elem* curr_disabled;
|
||||
|
||||
// Simultaneously traverse overlap and disabled,
|
||||
// and make collision queries only when required.
|
||||
int i;
|
||||
for ( i=0; i<osize; i++ )
|
||||
{ o1 = _set->get(i); //overlap[i]->id);
|
||||
if ( !o1 ) continue;
|
||||
if ( !o1->active ) continue; // this objects is not active
|
||||
|
||||
curr_disabled = i<dsize? disabled[i]:0;
|
||||
curr_overlap = overlap[i];
|
||||
|
||||
while ( curr_overlap )
|
||||
{ o2 = _set->get(curr_overlap->id);
|
||||
|
||||
// update curr_disabled position:
|
||||
while ( curr_disabled )
|
||||
{ if ( curr_disabled->id<curr_overlap->id )
|
||||
curr_disabled = curr_disabled->next;
|
||||
else break;
|
||||
}
|
||||
|
||||
call_collide = false;
|
||||
if ( !curr_disabled ) call_collide=true;
|
||||
else
|
||||
if ( curr_disabled->id>curr_overlap->id ) call_collide=true;
|
||||
else
|
||||
curr_disabled=curr_disabled->next;
|
||||
|
||||
//sr_out<<"COLLIDING "<<i<<" and "<<curr_overlap->id<<srnl;
|
||||
|
||||
if ( call_collide && o2->active )
|
||||
{
|
||||
_query.collide ( o1->R, o1->T, &(o1->tree), o2->R, o2->T, &(o2->tree), flag );
|
||||
_query.colliding_pairs().size();
|
||||
if ( _query.colliding_pairs().size()>0 ) // had collisions
|
||||
{ _pairs.push()=i;
|
||||
_pairs.push()=curr_overlap->id;
|
||||
if ( flag==SrBvTreeQuery::CollideFirstContact ) return true;
|
||||
}
|
||||
}
|
||||
curr_overlap = curr_overlap->next;
|
||||
}
|
||||
}
|
||||
return _pairs.size()>0? true:false;
|
||||
}
|
||||
|
||||
bool SrBvColdet::_collide_tolerance ( float toler )
|
||||
{
|
||||
const SrArray<SrBvIdPairs::Elem*>& overlap = _nbody.overlapping_pairs.elements();
|
||||
const SrArray<SrBvIdPairs::Elem*>& disabled = _disabled.elements();
|
||||
int osize = overlap.size();
|
||||
int dsize = disabled.size();
|
||||
|
||||
bool call_tolerance;
|
||||
Object* o1;
|
||||
Object* o2;
|
||||
const SrBvIdPairs::Elem* curr_overlap;
|
||||
const SrBvIdPairs::Elem* curr_disabled;
|
||||
|
||||
// Simultaneously traverse overlap and disabled,
|
||||
// and make collision queries only when required.
|
||||
int i;
|
||||
for ( i=0; i<osize; i++ )
|
||||
{ o1 = _set->get(i); //overlap[i]->id);
|
||||
if ( !o1 ) continue;
|
||||
if ( !o1->active ) continue; // this objects is not active
|
||||
|
||||
curr_disabled = i<dsize? disabled[i]:0;
|
||||
curr_overlap = overlap[i];
|
||||
|
||||
while ( curr_overlap )
|
||||
{ o2 = _set->get(curr_overlap->id);
|
||||
|
||||
// update curr_disabled position:
|
||||
while ( curr_disabled )
|
||||
{ if ( curr_disabled->id<curr_overlap->id )
|
||||
curr_disabled = curr_disabled->next;
|
||||
else break;
|
||||
}
|
||||
|
||||
call_tolerance = false;
|
||||
if ( !curr_disabled ) call_tolerance=true;
|
||||
else
|
||||
if ( curr_disabled->id>curr_overlap->id ) call_tolerance=true;
|
||||
else
|
||||
curr_disabled=curr_disabled->next;
|
||||
|
||||
if ( call_tolerance && o2->active )
|
||||
{ if ( _query.tolerance( o1->R, o1->T, &(o1->tree), o2->R, o2->T, &(o2->tree), toler ) )
|
||||
return true;
|
||||
}
|
||||
curr_overlap = curr_overlap->next;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
//=================================== EOF =====================================
|
||||
|
||||
95
source/dcdt/se/sr_bv_coldet.h
Normal file
95
source/dcdt/se/sr_bv_coldet.h
Normal file
@ -0,0 +1,95 @@
|
||||
/** \file sr_bv_coldet.h
|
||||
* multi model collision test */
|
||||
|
||||
# ifndef SR_BV_COLDET_H
|
||||
# define SR_BV_COLDET_H
|
||||
|
||||
|
||||
# include "precompiled.h"
|
||||
# include "sr_set.h"
|
||||
# include "sr_bv_nbody.h"
|
||||
# include "sr_bv_tree_query.h"
|
||||
|
||||
/* SrBvColdet uses SrBvNBody in order to efficiently determine
|
||||
collisions between several dynamic objects.
|
||||
Adapted from the VCollide package. Their copyright is in the source file. */
|
||||
class SrBvColdet
|
||||
{ private:
|
||||
class Object;
|
||||
friend class SrBvColdetObjectManager;
|
||||
SrSet<Object>* _set;
|
||||
SrBvNBody _nbody;
|
||||
SrBvIdPairs _disabled;
|
||||
SrBvTreeQuery _query;
|
||||
SrArray<int> _pairs;
|
||||
|
||||
public:
|
||||
/*! Constructor */
|
||||
SrBvColdet ();
|
||||
|
||||
/*! Destructor (calls init()) */
|
||||
~SrBvColdet ();
|
||||
|
||||
/*! Clears everything */
|
||||
void init ();
|
||||
|
||||
/*! Create a new object and returns its id. */
|
||||
int insert_object ( const SrModel& m );
|
||||
|
||||
/*! Removes (and deletes) the object from the database. */
|
||||
void remove_object ( int id );
|
||||
|
||||
/*! Returns true if id represents a valid id in the database, and false otherwise */
|
||||
bool id_valid ( int id ) { return _getobj(id)? true:false; }
|
||||
|
||||
/*! Update the transformation applied to an object.
|
||||
Only rotations and translations are supported. */
|
||||
void update_transformation ( int id, const SrMat& m );
|
||||
|
||||
/*! Turn on collision detection for an object. */
|
||||
void activate_object ( int id );
|
||||
|
||||
/*! Turn off collision detection for an object. */
|
||||
void deactivate_object ( int id );
|
||||
|
||||
/*! Turn on collision detection between a specific pair of objects. */
|
||||
void activate_pair ( int id1, int id2 );
|
||||
|
||||
/*! Turn off collision detection between a specific pair of objects. */
|
||||
void deactivate_pair ( int id1, int id2 );
|
||||
|
||||
/*! Returns true if the given pair of objects is deactivated and
|
||||
false otherwise (-1 ids return false). */
|
||||
bool pair_deactivated ( int id1, int id2 ) { return _disabled.pair_exists(id1,id2); }
|
||||
|
||||
/*! Counts and returns the number of deactivated pairs */
|
||||
int count_deactivated_pairs () { return _disabled.count_pairs(); }
|
||||
|
||||
/*! Returns the array with the ids of the colliding objects in
|
||||
the last collide_all() or has_collisions() query. The id
|
||||
pairs are sequentially stored in the array. Therefore, the number
|
||||
of collisions = colliding_pairs.size()/2. */
|
||||
const SrArray<int>& colliding_pairs () const { return _pairs; }
|
||||
|
||||
/*! Perform collision detection only untill finding a first collision.
|
||||
True is returned if a collision was found; false is returned otherwise. */
|
||||
bool collide ()
|
||||
{ return _collide ( SrBvTreeQuery::CollideFirstContact ); }
|
||||
|
||||
/*! Perform collision detection among all pairs of objects. The results can
|
||||
be retrieved with colliding_pairs() */
|
||||
bool collide_all ()
|
||||
{ return _collide ( SrBvTreeQuery::CollideAllContacts ); }
|
||||
|
||||
/*! Returns true as soon as the distance between one pair of models
|
||||
is found to be smaller than the given tolerance.
|
||||
False is returned when all pairs respects the minimum clearance. */
|
||||
bool collide_tolerance ( float toler ) { return _collide_tolerance(toler); }
|
||||
|
||||
private :
|
||||
bool _collide ( SrBvTreeQuery::CollideFlag flag );
|
||||
bool _collide_tolerance ( float toler );
|
||||
Object* _getobj ( int id ) { return ( id<0 || id>_set->maxid() )? 0:_set->get(id); }
|
||||
};
|
||||
|
||||
#endif // SR_BV_COLDET_H
|
||||
216
source/dcdt/se/sr_bv_id_pairs.cpp
Normal file
216
source/dcdt/se/sr_bv_id_pairs.cpp
Normal file
@ -0,0 +1,216 @@
|
||||
# include "precompiled.h"
|
||||
# include "sr_bv_id_pairs.h"
|
||||
|
||||
# define DEFAULT_SIZE 10
|
||||
|
||||
//============================== SrBvIdPairs ====================================
|
||||
|
||||
# define ORDER_IDS(id1,id2) { if (id1>id2) { SR_SWAPX(id1,id2); } } // make id1<id2
|
||||
|
||||
SrBvIdPairs::SrBvIdPairs ()
|
||||
{
|
||||
}
|
||||
|
||||
SrBvIdPairs::~SrBvIdPairs ()
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
void SrBvIdPairs::add_pair ( int id1, int id2 ) //add a pair to the set.
|
||||
{
|
||||
ORDER_IDS ( id1, id2 ); // order the ids
|
||||
if ( id1<0 ) return;
|
||||
|
||||
while ( id1>=_arr.size() ) // increase the size of "_arr"
|
||||
{ _arr.push() = 0; // reallocation is efficiently done when needed
|
||||
}
|
||||
|
||||
Elem *current = _arr[id1]; //select the right list from "_arr".
|
||||
|
||||
if ( !current ) //if the list is empty, insert the element in the front.
|
||||
{ current = new Elem;
|
||||
current->id = id2;
|
||||
current->next = NULL;
|
||||
_arr[id1] = current;
|
||||
}
|
||||
else if (current->id > id2) //if the list is not empty but all
|
||||
{ //elements are greater than id2, then
|
||||
current = new Elem; //insert id2 in the front.
|
||||
current->id = id2;
|
||||
current->next = _arr[id1];
|
||||
_arr[id1] = current;
|
||||
}
|
||||
else
|
||||
{ while (current->next != NULL) // otherwise, find the correct location
|
||||
{ // in the sorted list (ascending order)
|
||||
if (current->next->id > id2) //and insert id2 there.
|
||||
break;
|
||||
current = current->next;
|
||||
}
|
||||
if (current->id == id2) // already there
|
||||
{ return;
|
||||
}
|
||||
else
|
||||
{ Elem *temp = new Elem;
|
||||
temp->id = id2;
|
||||
temp->next = current->next;
|
||||
current->next = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrBvIdPairs::del_pair ( int id1, int id2 ) //delete a pair from the set.
|
||||
{
|
||||
ORDER_IDS(id1, id2); //order the ids.
|
||||
|
||||
if ( id1<0 || id1>=_arr.size() ) return; //the pair does not exist in the set, so return
|
||||
|
||||
Elem *current = _arr[id1]; //otherwise, select the correct list.
|
||||
|
||||
if ( !current ) return; //if this list is empty, the pair doesn't exist, so return
|
||||
|
||||
if (current->id == id2) //otherwise, if id2 is the first element, delete it
|
||||
{ _arr[id1] = current->next;
|
||||
delete current;
|
||||
return;
|
||||
}
|
||||
else
|
||||
{ while (current->next != NULL) //if id2 is not the first element,
|
||||
{ //start traversing the sorted list.
|
||||
if (current->next->id > id2) //if you have moved too far away
|
||||
{ //without hitting id2, then the pair
|
||||
return; //pair doesn't exist. So, return.
|
||||
}
|
||||
else if (current->next->id == id2) //otherwise, delete id2.
|
||||
{ Elem *temp = current->next;
|
||||
current->next = current->next->next;
|
||||
delete temp;
|
||||
return;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrBvIdPairs:: del_pairs_with_id ( int id ) //delete all pairs containing id.
|
||||
{
|
||||
int i;
|
||||
Elem *t;
|
||||
Elem *temp;
|
||||
|
||||
if ( id<_arr.size() )
|
||||
{ temp = _arr[id];
|
||||
while ( temp )
|
||||
{ t = temp;
|
||||
temp = temp->next;
|
||||
delete t;
|
||||
}
|
||||
_arr[id] = 0;
|
||||
|
||||
for ( i=0; i<id; i++ ) del_pair(i,id);
|
||||
}
|
||||
else
|
||||
{ for ( i=0; i<_arr.size(); i++ ) del_pair(i,id);
|
||||
}
|
||||
}
|
||||
|
||||
void SrBvIdPairs::init () //delete all pairs from the set.
|
||||
{
|
||||
int i;
|
||||
int size = _arr.size();
|
||||
Elem *current;
|
||||
|
||||
for ( i=0; i<size; i++ )
|
||||
{ while ( _arr[i] )
|
||||
{ current = _arr[i];
|
||||
_arr[i] = current->next;
|
||||
delete current;
|
||||
}
|
||||
}
|
||||
|
||||
_arr.size(0);
|
||||
};
|
||||
|
||||
bool SrBvIdPairs::pair_exists ( int id1, int id2 )
|
||||
{
|
||||
ORDER_IDS ( id1, id2 );
|
||||
|
||||
if ( id1>=_arr.size() || id1<0 || id2<0 ) return false; // the pair cannot exist
|
||||
|
||||
Elem *current = _arr[id1]; //otherwise, get the correct list and look for id2
|
||||
while ( current )
|
||||
{ if ( current->id==id2) return true;
|
||||
if ( current->id>id2) return false;
|
||||
current = current->next;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int SrBvIdPairs::count_pairs ()
|
||||
{
|
||||
int i, n = 0;
|
||||
|
||||
Elem *current;
|
||||
for ( i=0; i<_arr.size(); i++ )
|
||||
{ current = _arr[i];
|
||||
while ( current )
|
||||
{ n++; current = current->next; }
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
/************************************************************************\
|
||||
|
||||
Copyright 1997 The University of North Carolina at Chapel Hill.
|
||||
All Rights Reserved.
|
||||
|
||||
Permission to use, copy, modify and distribute this software
|
||||
and its documentation for educational, research and non-profit
|
||||
purposes, without fee, and without a written agreement is
|
||||
hereby granted, provided that the above copyright notice and
|
||||
the following three paragraphs appear in all copies.
|
||||
|
||||
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL
|
||||
HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL,
|
||||
INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS,
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
|
||||
EVEN IF THE UNIVERSITY OF NORTH CAROLINA HAVE BEEN ADVISED OF
|
||||
THE POSSIBILITY OF SUCH DAMAGES.
|
||||
|
||||
Permission to use, copy, modify and distribute this software
|
||||
and its documentation for educational, research and non-profit
|
||||
purposes, without fee, and without a written agreement is
|
||||
hereby granted, provided that the above copyright notice and
|
||||
the following three paragraphs appear in all copies.
|
||||
|
||||
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||||
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS"
|
||||
BASIS, AND THE UNIVERSITY OF NORTH CAROLINA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
||||
MODIFICATIONS.
|
||||
|
||||
|
||||
---------------------------------
|
||||
|Please send all BUG REPORTS to: |
|
||||
| |
|
||||
| geom@cs.unc.edu |
|
||||
| |
|
||||
---------------------------------
|
||||
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
US Mail: A. Pattekar/J. Cohen/T. Hudson/S. Gottschalk/M. Lin/D. Manocha
|
||||
Department of Computer Science
|
||||
Sitterson Hall, CB #3175
|
||||
University of N. Carolina
|
||||
Chapel Hill, NC 27599-3175
|
||||
|
||||
Phone: (919)962-1749
|
||||
|
||||
EMail: geom@cs.unc.edu
|
||||
|
||||
\************************************************************************/
|
||||
55
source/dcdt/se/sr_bv_id_pairs.h
Normal file
55
source/dcdt/se/sr_bv_id_pairs.h
Normal file
@ -0,0 +1,55 @@
|
||||
/** \file sr_bv_id_pairs.h
|
||||
* maintains a set of id pairs */
|
||||
|
||||
# ifndef SR_BV_ID_PAIRS_H
|
||||
# define SR_BV_ID_PAIRS_H
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
/*! Stores a set of pairs of integers. It is assumed that the data is sparse
|
||||
(i.e. the size of this set is small as compared to the n*(n-1)/2 possible pairs).
|
||||
A pair (id1, id2) exists <==> max(id1, id2) exists in the linked list pointed
|
||||
to by elements[min(id1, id2)].
|
||||
Adapted from the VCollide package. Their copyright is in the source file. */
|
||||
class SrBvIdPairs
|
||||
{ public:
|
||||
/*! The node of the linked list containing the 2nd ids of all pairs
|
||||
starting with the same id. It is a single connected list, with
|
||||
the last pointer as null */
|
||||
struct Elem { int id; Elem *next; };
|
||||
|
||||
private :
|
||||
SrArray<Elem*> _arr;
|
||||
|
||||
public :
|
||||
/*! Constructor */
|
||||
SrBvIdPairs ();
|
||||
|
||||
/*! Destructor */
|
||||
~SrBvIdPairs ();
|
||||
|
||||
/*! Returns the array of elements. Each element[i] points to the sorted list
|
||||
with all pairs (i,x) in the structure, i<x. If no pairs start with i,
|
||||
element[i] will be NULL, therefore some entries may have null pointers. */
|
||||
const SrArray<Elem*>& elements() const { return _arr; }
|
||||
|
||||
/*! Add a pair of ids to the set */
|
||||
void add_pair ( int id1, int id2 );
|
||||
|
||||
/*! Delete a pair from the set */
|
||||
void del_pair ( int id1, int id2 );
|
||||
|
||||
/*! Delete all pairs containing id */
|
||||
void del_pairs_with_id ( int id );
|
||||
|
||||
/*! Empty the set */
|
||||
void init ();
|
||||
|
||||
/*! Check if a pair of ids exists (-1 ids return false) */
|
||||
bool pair_exists ( int id1, int id2 );
|
||||
|
||||
/*! Counts and returns the number of existing pairs */
|
||||
int count_pairs ();
|
||||
};
|
||||
|
||||
#endif // SR_BV_ID_PAIRS_H
|
||||
902
source/dcdt/se/sr_bv_math.cpp
Normal file
902
source/dcdt/se/sr_bv_math.cpp
Normal file
@ -0,0 +1,902 @@
|
||||
#include "precompiled.h"/* Note: this code was adapted from the PQP library;
|
||||
their copyright notice can be found at the end of this file. */
|
||||
|
||||
# include <math.h>
|
||||
# include <stdio.h>
|
||||
|
||||
# include "sr_bv_math.h"
|
||||
|
||||
//====================== namespace SrBvMath =======================
|
||||
|
||||
/* void Mprint ( const srbvmat M );
|
||||
void SrBvMath::Mprint ( const srbvmat M )
|
||||
{
|
||||
printf("%g %g %g\n%g %g %g\n%g %g %g\n",
|
||||
M[0][0], M[0][1], M[0][2],
|
||||
M[1][0], M[1][1], M[1][2],
|
||||
M[2][0], M[2][1], M[2][2]);
|
||||
}*/
|
||||
|
||||
/* void Vprint ( const srbvvec V );
|
||||
void SrBvMath::Vprint ( const srbvvec V )
|
||||
{
|
||||
printf("%g %g %g\n", V[0], V[1], V[2]);
|
||||
}*/
|
||||
|
||||
/* void Vset ( srbvvec V, srbvreal x, srbvreal y, srbvreal z );
|
||||
void SrBvMath::Vset ( srbvvec V, srbvreal x, srbvreal y, srbvreal z )
|
||||
{
|
||||
V[0]=x; V[1]=y; V[2]=z;
|
||||
}*/
|
||||
|
||||
void SrBvMath::Vset ( srbvvec V, float* fp )
|
||||
{
|
||||
V[0]=fp[0]; V[1]=fp[1]; V[2]=fp[2];
|
||||
}
|
||||
|
||||
void SrBvMath::Midentity ( srbvmat M )
|
||||
{
|
||||
M[0][0] = M[1][1] = M[2][2] = 1;
|
||||
M[0][1] = M[1][2] = M[2][0] = 0;
|
||||
M[0][2] = M[1][0] = M[2][1] = 0;
|
||||
}
|
||||
|
||||
void SrBvMath::Videntity ( srbvvec T )
|
||||
{
|
||||
T[0] = T[1] = T[2] = 0.0;
|
||||
}
|
||||
|
||||
void SrBvMath::McM ( srbvmat Mr, const srbvmat M )
|
||||
{
|
||||
Mr[0][0] = M[0][0]; Mr[0][1] = M[0][1]; Mr[0][2] = M[0][2];
|
||||
Mr[1][0] = M[1][0]; Mr[1][1] = M[1][1]; Mr[1][2] = M[1][2];
|
||||
Mr[2][0] = M[2][0]; Mr[2][1] = M[2][1]; Mr[2][2] = M[2][2];
|
||||
}
|
||||
|
||||
/*void MTcM ( srbvmat Mr, const srbvmat M ); // Mr=M.transpose()
|
||||
void SrBvMath::MTcM ( srbvmat Mr, const srbvmat M )
|
||||
{
|
||||
Mr[0][0] = M[0][0]; Mr[1][0] = M[0][1]; Mr[2][0] = M[0][2];
|
||||
Mr[0][1] = M[1][0]; Mr[1][1] = M[1][1]; Mr[2][1] = M[1][2];
|
||||
Mr[0][2] = M[2][0]; Mr[1][2] = M[2][1]; Mr[2][2] = M[2][2];
|
||||
}*/
|
||||
|
||||
void SrBvMath::VcV ( srbvvec Vr, const srbvvec V )
|
||||
{
|
||||
Vr[0] = V[0]; Vr[1] = V[1]; Vr[2] = V[2];
|
||||
}
|
||||
|
||||
void SrBvMath::McolcV ( srbvvec Vr, const srbvmat M, int c )
|
||||
{
|
||||
Vr[0] = M[0][c];
|
||||
Vr[1] = M[1][c];
|
||||
Vr[2] = M[2][c];
|
||||
}
|
||||
|
||||
void SrBvMath::McolcMcol ( srbvmat Mr, int cr, const srbvmat M, int c )
|
||||
{
|
||||
Mr[0][cr] = M[0][c];
|
||||
Mr[1][cr] = M[1][c];
|
||||
Mr[2][cr] = M[2][c];
|
||||
}
|
||||
/*
|
||||
void MxMpV ( srbvmat Mr, const srbvmat M1, const srbvmat M2, const srbvvec T ); // Mr=M1*M2+T
|
||||
|
||||
void SrBvMath::MxMpV ( srbvmat Mr, const srbvmat M1, const srbvmat M2, const srbvvec T )
|
||||
{
|
||||
Mr[0][0] = (M1[0][0] * M2[0][0] +
|
||||
M1[0][1] * M2[1][0] +
|
||||
M1[0][2] * M2[2][0] +
|
||||
T[0]);
|
||||
Mr[1][0] = (M1[1][0] * M2[0][0] +
|
||||
M1[1][1] * M2[1][0] +
|
||||
M1[1][2] * M2[2][0] +
|
||||
T[1]);
|
||||
Mr[2][0] = (M1[2][0] * M2[0][0] +
|
||||
M1[2][1] * M2[1][0] +
|
||||
M1[2][2] * M2[2][0] +
|
||||
T[2]);
|
||||
Mr[0][1] = (M1[0][0] * M2[0][1] +
|
||||
M1[0][1] * M2[1][1] +
|
||||
M1[0][2] * M2[2][1] +
|
||||
T[0]);
|
||||
Mr[1][1] = (M1[1][0] * M2[0][1] +
|
||||
M1[1][1] * M2[1][1] +
|
||||
M1[1][2] * M2[2][1] +
|
||||
T[1]);
|
||||
Mr[2][1] = (M1[2][0] * M2[0][1] +
|
||||
M1[2][1] * M2[1][1] +
|
||||
M1[2][2] * M2[2][1] +
|
||||
T[2]);
|
||||
Mr[0][2] = (M1[0][0] * M2[0][2] +
|
||||
M1[0][1] * M2[1][2] +
|
||||
M1[0][2] * M2[2][2] +
|
||||
T[0]);
|
||||
Mr[1][2] = (M1[1][0] * M2[0][2] +
|
||||
M1[1][1] * M2[1][2] +
|
||||
M1[1][2] * M2[2][2] +
|
||||
T[1]);
|
||||
Mr[2][2] = (M1[2][0] * M2[0][2] +
|
||||
M1[2][1] * M2[1][2] +
|
||||
M1[2][2] * M2[2][2] +
|
||||
T[2]);
|
||||
}*/
|
||||
|
||||
void SrBvMath::MxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 )
|
||||
{
|
||||
Mr[0][0] = (M1[0][0] * M2[0][0] +
|
||||
M1[0][1] * M2[1][0] +
|
||||
M1[0][2] * M2[2][0]);
|
||||
Mr[1][0] = (M1[1][0] * M2[0][0] +
|
||||
M1[1][1] * M2[1][0] +
|
||||
M1[1][2] * M2[2][0]);
|
||||
Mr[2][0] = (M1[2][0] * M2[0][0] +
|
||||
M1[2][1] * M2[1][0] +
|
||||
M1[2][2] * M2[2][0]);
|
||||
Mr[0][1] = (M1[0][0] * M2[0][1] +
|
||||
M1[0][1] * M2[1][1] +
|
||||
M1[0][2] * M2[2][1]);
|
||||
Mr[1][1] = (M1[1][0] * M2[0][1] +
|
||||
M1[1][1] * M2[1][1] +
|
||||
M1[1][2] * M2[2][1]);
|
||||
Mr[2][1] = (M1[2][0] * M2[0][1] +
|
||||
M1[2][1] * M2[1][1] +
|
||||
M1[2][2] * M2[2][1]);
|
||||
Mr[0][2] = (M1[0][0] * M2[0][2] +
|
||||
M1[0][1] * M2[1][2] +
|
||||
M1[0][2] * M2[2][2]);
|
||||
Mr[1][2] = (M1[1][0] * M2[0][2] +
|
||||
M1[1][1] * M2[1][2] +
|
||||
M1[1][2] * M2[2][2]);
|
||||
Mr[2][2] = (M1[2][0] * M2[0][2] +
|
||||
M1[2][1] * M2[1][2] +
|
||||
M1[2][2] * M2[2][2]);
|
||||
}
|
||||
|
||||
/* void MxMT ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1*M2.transpose()
|
||||
void SrBvMath::MxMT ( srbvmat Mr, const srbvmat M1, const srbvmat M2 )
|
||||
{
|
||||
Mr[0][0] = (M1[0][0] * M2[0][0] +
|
||||
M1[0][1] * M2[0][1] +
|
||||
M1[0][2] * M2[0][2]);
|
||||
Mr[1][0] = (M1[1][0] * M2[0][0] +
|
||||
M1[1][1] * M2[0][1] +
|
||||
M1[1][2] * M2[0][2]);
|
||||
Mr[2][0] = (M1[2][0] * M2[0][0] +
|
||||
M1[2][1] * M2[0][1] +
|
||||
M1[2][2] * M2[0][2]);
|
||||
Mr[0][1] = (M1[0][0] * M2[1][0] +
|
||||
M1[0][1] * M2[1][1] +
|
||||
M1[0][2] * M2[1][2]);
|
||||
Mr[1][1] = (M1[1][0] * M2[1][0] +
|
||||
M1[1][1] * M2[1][1] +
|
||||
M1[1][2] * M2[1][2]);
|
||||
Mr[2][1] = (M1[2][0] * M2[1][0] +
|
||||
M1[2][1] * M2[1][1] +
|
||||
M1[2][2] * M2[1][2]);
|
||||
Mr[0][2] = (M1[0][0] * M2[2][0] +
|
||||
M1[0][1] * M2[2][1] +
|
||||
M1[0][2] * M2[2][2]);
|
||||
Mr[1][2] = (M1[1][0] * M2[2][0] +
|
||||
M1[1][1] * M2[2][1] +
|
||||
M1[1][2] * M2[2][2]);
|
||||
Mr[2][2] = (M1[2][0] * M2[2][0] +
|
||||
M1[2][1] * M2[2][1] +
|
||||
M1[2][2] * M2[2][2]);
|
||||
}*/
|
||||
|
||||
void SrBvMath::MTxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 )
|
||||
{
|
||||
Mr[0][0] = (M1[0][0] * M2[0][0] +
|
||||
M1[1][0] * M2[1][0] +
|
||||
M1[2][0] * M2[2][0]);
|
||||
Mr[1][0] = (M1[0][1] * M2[0][0] +
|
||||
M1[1][1] * M2[1][0] +
|
||||
M1[2][1] * M2[2][0]);
|
||||
Mr[2][0] = (M1[0][2] * M2[0][0] +
|
||||
M1[1][2] * M2[1][0] +
|
||||
M1[2][2] * M2[2][0]);
|
||||
Mr[0][1] = (M1[0][0] * M2[0][1] +
|
||||
M1[1][0] * M2[1][1] +
|
||||
M1[2][0] * M2[2][1]);
|
||||
Mr[1][1] = (M1[0][1] * M2[0][1] +
|
||||
M1[1][1] * M2[1][1] +
|
||||
M1[2][1] * M2[2][1]);
|
||||
Mr[2][1] = (M1[0][2] * M2[0][1] +
|
||||
M1[1][2] * M2[1][1] +
|
||||
M1[2][2] * M2[2][1]);
|
||||
Mr[0][2] = (M1[0][0] * M2[0][2] +
|
||||
M1[1][0] * M2[1][2] +
|
||||
M1[2][0] * M2[2][2]);
|
||||
Mr[1][2] = (M1[0][1] * M2[0][2] +
|
||||
M1[1][1] * M2[1][2] +
|
||||
M1[2][1] * M2[2][2]);
|
||||
Mr[2][2] = (M1[0][2] * M2[0][2] +
|
||||
M1[1][2] * M2[1][2] +
|
||||
M1[2][2] * M2[2][2]);
|
||||
}
|
||||
|
||||
void SrBvMath::MxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 )
|
||||
{
|
||||
Vr[0] = (M1[0][0] * V1[0] +
|
||||
M1[0][1] * V1[1] +
|
||||
M1[0][2] * V1[2]);
|
||||
Vr[1] = (M1[1][0] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[1][2] * V1[2]);
|
||||
Vr[2] = (M1[2][0] * V1[0] +
|
||||
M1[2][1] * V1[1] +
|
||||
M1[2][2] * V1[2]);
|
||||
}
|
||||
|
||||
void SrBvMath::MxVpV ( srbvvec Vr, const srbvmat M1, const srbvvec V1, const srbvvec V2)
|
||||
{
|
||||
Vr[0] = (M1[0][0] * V1[0] +
|
||||
M1[0][1] * V1[1] +
|
||||
M1[0][2] * V1[2] +
|
||||
V2[0]);
|
||||
Vr[1] = (M1[1][0] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[1][2] * V1[2] +
|
||||
V2[1]);
|
||||
Vr[2] = (M1[2][0] * V1[0] +
|
||||
M1[2][1] * V1[1] +
|
||||
M1[2][2] * V1[2] +
|
||||
V2[2]);
|
||||
}
|
||||
|
||||
/* void sMxVpV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1, const srbvvec V2 );
|
||||
void SrBvMath::sMxVpV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
Vr[0] = s1 * (M1[0][0] * V1[0] +
|
||||
M1[0][1] * V1[1] +
|
||||
M1[0][2] * V1[2]) +
|
||||
V2[0];
|
||||
Vr[1] = s1 * (M1[1][0] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[1][2] * V1[2]) +
|
||||
V2[1];
|
||||
Vr[2] = s1 * (M1[2][0] * V1[0] +
|
||||
M1[2][1] * V1[1] +
|
||||
M1[2][2] * V1[2]) +
|
||||
V2[2];
|
||||
}*/
|
||||
|
||||
void SrBvMath::MTxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 )
|
||||
{
|
||||
Vr[0] = (M1[0][0] * V1[0] +
|
||||
M1[1][0] * V1[1] +
|
||||
M1[2][0] * V1[2]);
|
||||
Vr[1] = (M1[0][1] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[2][1] * V1[2]);
|
||||
Vr[2] = (M1[0][2] * V1[0] +
|
||||
M1[1][2] * V1[1] +
|
||||
M1[2][2] * V1[2]);
|
||||
}
|
||||
|
||||
/* void sMTxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 );
|
||||
void SrBvMath::sMTxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 )
|
||||
{
|
||||
Vr[0] = s1*(M1[0][0] * V1[0] +
|
||||
M1[1][0] * V1[1] +
|
||||
M1[2][0] * V1[2]);
|
||||
Vr[1] = s1*(M1[0][1] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[2][1] * V1[2]);
|
||||
Vr[2] = s1*(M1[0][2] * V1[0] +
|
||||
M1[1][2] * V1[1] +
|
||||
M1[2][2] * V1[2]);
|
||||
}*/
|
||||
|
||||
/* void sMxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 );
|
||||
void SrBvMath::sMxV ( srbvvec Vr, srbvreal s1, const srbvmat M1, const srbvvec V1 )
|
||||
{
|
||||
Vr[0] = s1*(M1[0][0] * V1[0] +
|
||||
M1[0][1] * V1[1] +
|
||||
M1[0][2] * V1[2]);
|
||||
Vr[1] = s1*(M1[1][0] * V1[0] +
|
||||
M1[1][1] * V1[1] +
|
||||
M1[1][2] * V1[2]);
|
||||
Vr[2] = s1*(M1[2][0] * V1[0] +
|
||||
M1[2][1] * V1[1] +
|
||||
M1[2][2] * V1[2]);
|
||||
}*/
|
||||
|
||||
void SrBvMath::VmV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
Vr[0] = V1[0] - V2[0];
|
||||
Vr[1] = V1[1] - V2[1];
|
||||
Vr[2] = V1[2] - V2[2];
|
||||
}
|
||||
|
||||
void SrBvMath::VpV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
Vr[0] = V1[0] + V2[0];
|
||||
Vr[1] = V1[1] + V2[1];
|
||||
Vr[2] = V1[2] + V2[2];
|
||||
}
|
||||
|
||||
void SrBvMath::VpVxS ( srbvvec Vr, const srbvvec V1, const srbvvec V2, srbvreal s )
|
||||
{
|
||||
Vr[0] = V1[0] + V2[0] * s;
|
||||
Vr[1] = V1[1] + V2[1] * s;
|
||||
Vr[2] = V1[2] + V2[2] * s;
|
||||
}
|
||||
|
||||
/* void MskewV ( srbvmat M, const srbvvec v );
|
||||
void SrBvMath::MskewV ( srbvmat M, const srbvvec v )
|
||||
{
|
||||
M[0][0] = M[1][1] = M[2][2] = 0.0;
|
||||
M[1][0] = v[2];
|
||||
M[0][1] = -v[2];
|
||||
M[0][2] = v[1];
|
||||
M[2][0] = -v[1];
|
||||
M[1][2] = -v[0];
|
||||
M[2][1] = v[0];
|
||||
}*/
|
||||
|
||||
void SrBvMath::VcrossV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
Vr[0] = V1[1]*V2[2] - V1[2]*V2[1];
|
||||
Vr[1] = V1[2]*V2[0] - V1[0]*V2[2];
|
||||
Vr[2] = V1[0]*V2[1] - V1[1]*V2[0];
|
||||
}
|
||||
|
||||
/* srbvreal Vlength ( srbvvec V );
|
||||
srbvreal SrBvMath::Vlength ( srbvvec V )
|
||||
{
|
||||
return sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
|
||||
}*/
|
||||
|
||||
/* void Vnormalize ( srbvvec V );
|
||||
void SrBvMath::Vnormalize ( srbvvec V )
|
||||
{
|
||||
srbvreal d = srbvreal(1.0) / sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]);
|
||||
V[0] *= d;
|
||||
V[1] *= d;
|
||||
V[2] *= d;
|
||||
}*/
|
||||
|
||||
srbvreal SrBvMath::VdotV ( const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
return (V1[0]*V2[0] + V1[1]*V2[1] + V1[2]*V2[2]);
|
||||
}
|
||||
|
||||
|
||||
srbvreal SrBvMath::VdistV2 ( const srbvvec V1, const srbvvec V2 )
|
||||
{
|
||||
return ( (V1[0]-V2[0]) * (V1[0]-V2[0]) +
|
||||
(V1[1]-V2[1]) * (V1[1]-V2[1]) +
|
||||
(V1[2]-V2[2]) * (V1[2]-V2[2]));
|
||||
}
|
||||
|
||||
|
||||
void SrBvMath::VxS ( srbvvec Vr, const srbvvec V, srbvreal s )
|
||||
{
|
||||
Vr[0] = V[0] * s;
|
||||
Vr[1] = V[1] * s;
|
||||
Vr[2] = V[2] * s;
|
||||
}
|
||||
|
||||
/* void MRotZ ( srbvmat Mr, srbvreal t );
|
||||
void MRotX ( srbvmat Mr, srbvreal t );
|
||||
void MRotY ( srbvmat Mr, srbvreal t );
|
||||
void Mqinverse ( srbvmat Mr, srbvmat M );*/
|
||||
/*
|
||||
void SrBvMath::MRotZ ( srbvmat Mr, srbvreal t)
|
||||
{
|
||||
Mr[0][0] = cos(t);
|
||||
Mr[1][0] = sin(t);
|
||||
Mr[0][1] = -Mr[1][0];
|
||||
Mr[1][1] = Mr[0][0];
|
||||
Mr[2][0] = Mr[2][1] = 0.0;
|
||||
Mr[0][2] = Mr[1][2] = 0.0;
|
||||
Mr[2][2] = 1.0;
|
||||
}
|
||||
|
||||
void SrBvMath::MRotX ( srbvmat Mr, srbvreal t )
|
||||
{
|
||||
Mr[1][1] = cos(t);
|
||||
Mr[2][1] = sin(t);
|
||||
Mr[1][2] = -Mr[2][1];
|
||||
Mr[2][2] = Mr[1][1];
|
||||
Mr[0][1] = Mr[0][2] = 0.0;
|
||||
Mr[1][0] = Mr[2][0] = 0.0;
|
||||
Mr[0][0] = 1.0;
|
||||
}
|
||||
|
||||
void SrBvMath::MRotY ( srbvmat Mr, srbvreal t )
|
||||
{
|
||||
Mr[2][2] = cos(t);
|
||||
Mr[0][2] = sin(t);
|
||||
Mr[2][0] = -Mr[0][2];
|
||||
Mr[0][0] = Mr[2][2];
|
||||
Mr[1][2] = Mr[1][0] = 0.0;
|
||||
Mr[2][1] = Mr[0][1] = 0.0;
|
||||
Mr[1][1] = 1.0;
|
||||
}
|
||||
|
||||
void SrBvMath::Mqinverse ( srbvmat Mr, srbvmat M )
|
||||
{
|
||||
int i,j;
|
||||
|
||||
for(i=0; i<3; i++)
|
||||
for(j=0; j<3; j++)
|
||||
{
|
||||
int i1 = (i+1)%3;
|
||||
int i2 = (i+2)%3;
|
||||
int j1 = (j+1)%3;
|
||||
int j2 = (j+2)%3;
|
||||
Mr[i][j] = (M[j1][i1]*M[j2][i2] - M[j1][i2]*M[j2][i1]);
|
||||
}
|
||||
}*/
|
||||
|
||||
// Meigen from Numerical Recipes in C
|
||||
|
||||
#define ROTATE(a,i,j,k,l) g=a[i][j]; h=a[k][l]; a[i][j]=g-s*(h+g*tau); a[k][l]=h+s*(g-h*tau)
|
||||
|
||||
void SrBvMath::Meigen ( srbvmat vout, srbvvec dout, srbvmat a )
|
||||
{
|
||||
int n = 3;
|
||||
int j,iq,ip,i;
|
||||
srbvreal tresh,theta,tau,t,sm,s,h,g,c;
|
||||
int nrot;
|
||||
srbvvec b, z, d;
|
||||
srbvmat v;
|
||||
|
||||
Midentity(v);
|
||||
for(ip=0; ip<n; ip++)
|
||||
{
|
||||
b[ip] = a[ip][ip];
|
||||
d[ip] = a[ip][ip];
|
||||
z[ip] = 0.0;
|
||||
}
|
||||
|
||||
nrot = 0;
|
||||
|
||||
for(i=0; i<50; i++)
|
||||
{ sm=0.0;
|
||||
for(ip=0;ip<n;ip++) for(iq=ip+1;iq<n;iq++) sm+=fabs(a[ip][iq]);
|
||||
if (sm == 0.0)
|
||||
{ McM(vout, v);
|
||||
VcV(dout, d);
|
||||
return;
|
||||
}
|
||||
if (i < 3)
|
||||
tresh=srbvreal(0.2)*sm/(n*n);
|
||||
else
|
||||
tresh=0.0;
|
||||
|
||||
for(ip=0; ip<n; ip++) for(iq=ip+1; iq<n; iq++)
|
||||
{ g = srbvreal(100.0)*fabs(a[ip][iq]);
|
||||
if (i>3 &&
|
||||
fabs(d[ip])+g==fabs(d[ip]) &&
|
||||
fabs(d[iq])+g==fabs(d[iq]))
|
||||
{ a[ip][iq]=0.0; }
|
||||
else if (fabs(a[ip][iq])>tresh)
|
||||
{ h = d[iq]-d[ip];
|
||||
if (fabs(h)+g == fabs(h)) t=(a[ip][iq])/h;
|
||||
else
|
||||
{ theta=(srbvreal)0.5*h/(a[ip][iq]);
|
||||
t=(srbvreal)(1.0/(fabs(theta)+sqrt(1.0+theta*theta)));
|
||||
if (theta < 0.0) t = -t;
|
||||
}
|
||||
c=(srbvreal)1.0/sqrt(1+t*t);
|
||||
s=t*c;
|
||||
tau=s/((srbvreal)1.0+c);
|
||||
h=t*a[ip][iq];
|
||||
z[ip] -= h;
|
||||
z[iq] += h;
|
||||
d[ip] -= h;
|
||||
d[iq] += h;
|
||||
a[ip][iq]=0.0;
|
||||
for(j=0;j<ip;j++) { ROTATE(a,j,ip,j,iq); }
|
||||
for(j=ip+1;j<iq;j++) { ROTATE(a,ip,j,j,iq); }
|
||||
for(j=iq+1;j<n;j++) { ROTATE(a,ip,j,iq,j); }
|
||||
for(j=0;j<n;j++) { ROTATE(v,j,ip,j,iq); }
|
||||
nrot++;
|
||||
}
|
||||
}
|
||||
for(ip=0;ip<n;ip++)
|
||||
{ b[ip] += z[ip];
|
||||
d[ip] = b[ip];
|
||||
z[ip] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
fprintf(stderr, "eigen: too many iterations in Jacobi transform.\n");
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <float.h>
|
||||
#define isnan _isnan
|
||||
#endif
|
||||
|
||||
//--------------------------------------------------------------------------
|
||||
// SegPoints()
|
||||
//
|
||||
// Returns closest points between an segment pair.
|
||||
// Implemented from an algorithm described in
|
||||
//
|
||||
// Vladimir J. Lumelsky,
|
||||
// On fast computation of distance between line segments.
|
||||
// In Information Processing Letters, no. 21, pages 55-61, 1985.
|
||||
//--------------------------------------------------------------------------
|
||||
void SrBvMath::SegPoints ( srbvvec VEC, srbvvec X, srbvvec Y,
|
||||
const srbvvec P, const srbvvec A,
|
||||
const srbvvec Q, const srbvvec B )
|
||||
{
|
||||
srbvreal T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
|
||||
srbvreal TMP[3];
|
||||
|
||||
VmV(T,Q,P);
|
||||
A_dot_A = VdotV(A,A);
|
||||
B_dot_B = VdotV(B,B);
|
||||
A_dot_B = VdotV(A,B);
|
||||
A_dot_T = VdotV(A,T);
|
||||
B_dot_T = VdotV(B,T);
|
||||
|
||||
// t parameterizes ray P,A
|
||||
// u parameterizes ray Q,B
|
||||
|
||||
srbvreal t,u;
|
||||
|
||||
// compute t for the closest point on ray P,A to
|
||||
// ray Q,B
|
||||
|
||||
srbvreal denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
|
||||
|
||||
t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
|
||||
|
||||
// clamp result so t is on the segment P,A
|
||||
|
||||
if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1;
|
||||
|
||||
// find u for point on ray Q,B closest to point at t
|
||||
|
||||
u = (t*A_dot_B - B_dot_T) / B_dot_B;
|
||||
|
||||
// if u is on segment Q,B, t and u correspond to
|
||||
// closest points, otherwise, clamp u, recompute and
|
||||
// clamp t
|
||||
|
||||
if ((u <= 0) || isnan(u)) {
|
||||
|
||||
VcV(Y, Q);
|
||||
|
||||
t = A_dot_T / A_dot_A;
|
||||
|
||||
if ((t <= 0) || isnan(t)) {
|
||||
VcV(X, P);
|
||||
VmV(VEC, Q, P);
|
||||
}
|
||||
else if (t >= 1) {
|
||||
VpV(X, P, A);
|
||||
VmV(VEC, Q, X);
|
||||
}
|
||||
else {
|
||||
VpVxS(X, P, A, t);
|
||||
VcrossV(TMP, T, A);
|
||||
VcrossV(VEC, A, TMP);
|
||||
}
|
||||
}
|
||||
else if (u >= 1) {
|
||||
|
||||
VpV(Y, Q, B);
|
||||
|
||||
t = (A_dot_B + A_dot_T) / A_dot_A;
|
||||
|
||||
if ((t <= 0) || isnan(t)) {
|
||||
VcV(X, P);
|
||||
VmV(VEC, Y, P);
|
||||
}
|
||||
else if (t >= 1) {
|
||||
VpV(X, P, A);
|
||||
VmV(VEC, Y, X);
|
||||
}
|
||||
else {
|
||||
VpVxS(X, P, A, t);
|
||||
VmV(T, Y, P);
|
||||
VcrossV(TMP, T, A);
|
||||
VcrossV(VEC, A, TMP);
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
||||
VpVxS(Y, Q, B, u);
|
||||
|
||||
if ((t <= 0) || isnan(t)) {
|
||||
VcV(X, P);
|
||||
VcrossV(TMP, T, B);
|
||||
VcrossV(VEC, B, TMP);
|
||||
}
|
||||
else if (t >= 1) {
|
||||
VpV(X, P, A);
|
||||
VmV(T, Q, X);
|
||||
VcrossV(TMP, T, B);
|
||||
VcrossV(VEC, B, TMP);
|
||||
}
|
||||
else {
|
||||
VpVxS(X, P, A, t);
|
||||
VcrossV(VEC, A, B);
|
||||
if (VdotV(VEC, T) < 0) {
|
||||
VxS(VEC, VEC, -1);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------------------
|
||||
// TriDist()
|
||||
//
|
||||
// Computes the closest points on two triangles, and returns the
|
||||
// distance between them.
|
||||
//
|
||||
// S and T are the triangles, stored tri[point][dimension].
|
||||
//
|
||||
// If the triangles are disjoint, P and Q give the closest points of
|
||||
// S and T respectively. However, if the triangles overlap, P and Q
|
||||
// are basically a random pair of points from the triangles, not
|
||||
// coincident points on the intersection of the triangles, as might
|
||||
// be expected.
|
||||
//--------------------------------------------------------------------------
|
||||
srbvreal
|
||||
SrBvMath::TriDist( srbvvec P, srbvvec Q,
|
||||
const srbvmat S, const srbvmat T )
|
||||
{
|
||||
// Compute vectors along the 6 sides
|
||||
|
||||
srbvreal Sv[3][3], Tv[3][3];
|
||||
srbvreal VEC[3];
|
||||
|
||||
VmV(Sv[0],S[1],S[0]);
|
||||
VmV(Sv[1],S[2],S[1]);
|
||||
VmV(Sv[2],S[0],S[2]);
|
||||
|
||||
VmV(Tv[0],T[1],T[0]);
|
||||
VmV(Tv[1],T[2],T[1]);
|
||||
VmV(Tv[2],T[0],T[2]);
|
||||
|
||||
// For each edge pair, the vector connecting the closest points
|
||||
// of the edges defines a slab (parallel planes at head and tail
|
||||
// enclose the slab). If we can show that the off-edge vertex of
|
||||
// each triangle is outside of the slab, then the closest points
|
||||
// of the edges are the closest points for the triangles.
|
||||
// Even if these tests fail, it may be helpful to know the closest
|
||||
// points found, and whether the triangles were shown disjoint
|
||||
|
||||
srbvreal V[3];
|
||||
srbvreal Z[3];
|
||||
srbvreal minP[3], minQ[3], mindd;
|
||||
int shown_disjoint = 0;
|
||||
|
||||
mindd = VdistV2(S[0],T[0]) + 1; // Set first minimum safely high
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
// Find closest points on edges i & j, plus the
|
||||
// vector (and distance squared) between these points
|
||||
|
||||
SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]);
|
||||
|
||||
VmV(V,Q,P);
|
||||
srbvreal dd = VdotV(V,V);
|
||||
|
||||
// Verify this closest point pair only if the distance
|
||||
// squared is less than the minimum found thus far.
|
||||
|
||||
if (dd <= mindd)
|
||||
{
|
||||
VcV(minP,P);
|
||||
VcV(minQ,Q);
|
||||
mindd = dd;
|
||||
|
||||
VmV(Z,S[(i+2)%3],P);
|
||||
srbvreal a = VdotV(Z,VEC);
|
||||
VmV(Z,T[(j+2)%3],Q);
|
||||
srbvreal b = VdotV(Z,VEC);
|
||||
|
||||
if ((a <= 0) && (b >= 0)) return sqrt(dd);
|
||||
|
||||
srbvreal p = VdotV(V, VEC);
|
||||
|
||||
if (a < 0) a = 0;
|
||||
if (b > 0) b = 0;
|
||||
if ((p - a + b) > 0) shown_disjoint = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// No edge pairs contained the closest points.
|
||||
// either:
|
||||
// 1. one of the closest points is a vertex, and the
|
||||
// other point is interior to a face.
|
||||
// 2. the triangles are overlapping.
|
||||
// 3. an edge of one triangle is parallel to the other's face. If
|
||||
// cases 1 and 2 are not true, then the closest points from the 9
|
||||
// edge pairs checks above can be taken as closest points for the
|
||||
// triangles.
|
||||
// 4. possibly, the triangles were degenerate. When the
|
||||
// triangle points are nearly colinear or coincident, one
|
||||
// of above tests might fail even though the edges tested
|
||||
// contain the closest points.
|
||||
|
||||
// First check for case 1
|
||||
|
||||
srbvreal Sn[3], Snl;
|
||||
VcrossV(Sn,Sv[0],Sv[1]); // Compute normal to S triangle
|
||||
Snl = VdotV(Sn,Sn); // Compute square of length of normal
|
||||
|
||||
// If cross product is long enough,
|
||||
|
||||
if (Snl > 1e-15)
|
||||
{
|
||||
// Get projection lengths of T points
|
||||
|
||||
srbvreal Tp[3];
|
||||
|
||||
VmV(V,S[0],T[0]);
|
||||
Tp[0] = VdotV(V,Sn);
|
||||
|
||||
VmV(V,S[0],T[1]);
|
||||
Tp[1] = VdotV(V,Sn);
|
||||
|
||||
VmV(V,S[0],T[2]);
|
||||
Tp[2] = VdotV(V,Sn);
|
||||
|
||||
// If Sn is a separating direction,
|
||||
// find point with smallest projection
|
||||
|
||||
int point = -1;
|
||||
if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
|
||||
{
|
||||
if (Tp[0] < Tp[1]) point = 0; else point = 1;
|
||||
if (Tp[2] < Tp[point]) point = 2;
|
||||
}
|
||||
else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
|
||||
{
|
||||
if (Tp[0] > Tp[1]) point = 0; else point = 1;
|
||||
if (Tp[2] > Tp[point]) point = 2;
|
||||
}
|
||||
|
||||
// If Sn is a separating direction,
|
||||
|
||||
if (point >= 0)
|
||||
{
|
||||
shown_disjoint = 1;
|
||||
|
||||
// Test whether the point found, when projected onto the
|
||||
// other triangle, lies within the face.
|
||||
|
||||
VmV(V,T[point],S[0]);
|
||||
VcrossV(Z,Sn,Sv[0]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
VmV(V,T[point],S[1]);
|
||||
VcrossV(Z,Sn,Sv[1]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
VmV(V,T[point],S[2]);
|
||||
VcrossV(Z,Sn,Sv[2]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
// T[point] passed the test - it's a closest point for
|
||||
// the T triangle; the other point is on the face of S
|
||||
|
||||
VpVxS(P,T[point],Sn,Tp[point]/Snl);
|
||||
VcV(Q,T[point]);
|
||||
return sqrt(VdistV2(P,Q));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
srbvreal Tn[3], Tnl;
|
||||
VcrossV(Tn,Tv[0],Tv[1]);
|
||||
Tnl = VdotV(Tn,Tn);
|
||||
|
||||
if (Tnl > 1e-15)
|
||||
{
|
||||
srbvreal Sp[3];
|
||||
|
||||
VmV(V,T[0],S[0]);
|
||||
Sp[0] = VdotV(V,Tn);
|
||||
|
||||
VmV(V,T[0],S[1]);
|
||||
Sp[1] = VdotV(V,Tn);
|
||||
|
||||
VmV(V,T[0],S[2]);
|
||||
Sp[2] = VdotV(V,Tn);
|
||||
|
||||
int point = -1;
|
||||
if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
|
||||
{
|
||||
if (Sp[0] < Sp[1]) point = 0; else point = 1;
|
||||
if (Sp[2] < Sp[point]) point = 2;
|
||||
}
|
||||
else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
|
||||
{
|
||||
if (Sp[0] > Sp[1]) point = 0; else point = 1;
|
||||
if (Sp[2] > Sp[point]) point = 2;
|
||||
}
|
||||
|
||||
if (point >= 0)
|
||||
{
|
||||
shown_disjoint = 1;
|
||||
|
||||
VmV(V,S[point],T[0]);
|
||||
VcrossV(Z,Tn,Tv[0]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
VmV(V,S[point],T[1]);
|
||||
VcrossV(Z,Tn,Tv[1]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
VmV(V,S[point],T[2]);
|
||||
VcrossV(Z,Tn,Tv[2]);
|
||||
if (VdotV(V,Z) > 0)
|
||||
{
|
||||
VcV(P,S[point]);
|
||||
VpVxS(Q,S[point],Tn,Sp[point]/Tnl);
|
||||
return sqrt(VdistV2(P,Q));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Case 1 can't be shown.
|
||||
// If one of these tests showed the triangles disjoint,
|
||||
// we assume case 3 or 4, otherwise we conclude case 2,
|
||||
// that the triangles overlap.
|
||||
|
||||
if (shown_disjoint)
|
||||
{
|
||||
VcV(P,minP);
|
||||
VcV(Q,minQ);
|
||||
return sqrt(mindd);
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
/*************************************************************************\
|
||||
Copyright 1999 The University of North Carolina at Chapel Hill.
|
||||
All Rights Reserved.
|
||||
|
||||
Permission to use, copy, modify and distribute this software and its
|
||||
documentation for educational, research and non-profit purposes, without
|
||||
fee, and without a written agreement is hereby granted, provided that the
|
||||
above copyright notice and the following three paragraphs appear in all
|
||||
copies.
|
||||
|
||||
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
|
||||
LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
|
||||
CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
|
||||
USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
|
||||
OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
|
||||
DAMAGES.
|
||||
|
||||
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||||
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
|
||||
PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
|
||||
NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
|
||||
UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
US Mail: S. Gottschalk, E. Larsen
|
||||
Department of Computer Science
|
||||
Sitterson Hall, CB #3175
|
||||
University of N. Carolina
|
||||
Chapel Hill, NC 27599-3175
|
||||
Phone: (919)962-1749
|
||||
EMail: geom@cs.unc.edu
|
||||
\**************************************************************************/
|
||||
|
||||
72
source/dcdt/se/sr_bv_math.h
Normal file
72
source/dcdt/se/sr_bv_math.h
Normal file
@ -0,0 +1,72 @@
|
||||
/** \file sr_bv_math.h
|
||||
* BV tree math routines */
|
||||
|
||||
# ifndef SR_BV_MATH_H
|
||||
# define SR_BV_MATH_H
|
||||
|
||||
# include "sr.h"
|
||||
|
||||
/*! if the following macro is defined, will use floats instead of doubles
|
||||
Default is doubles (macro not defined) */
|
||||
# ifdef SR_BV_MATH_FLOAT
|
||||
typedef float srbvreal;
|
||||
# else
|
||||
typedef double srbvreal;
|
||||
# endif
|
||||
|
||||
typedef srbvreal srbvmat[3][3];
|
||||
typedef srbvreal srbvvec[3];
|
||||
|
||||
const srbvreal srbvpi = (srbvreal)SR_PI;
|
||||
|
||||
//============================== SrBvMath ===============================
|
||||
|
||||
/*! This namespace encapsulates a (row-major) matrix/vector code
|
||||
adapted from the PQP package. Their copyright notice
|
||||
is displayed in the source file */
|
||||
namespace SrBvMath
|
||||
{
|
||||
void Vset ( srbvvec V, float* fp );
|
||||
void Midentity ( srbvmat M ); // set to id matrix
|
||||
void Videntity ( srbvvec T ); // set to a null vector
|
||||
void McM ( srbvmat Mr, const srbvmat M ); // Mr=M
|
||||
void VcV ( srbvvec Vr, const srbvvec V ); // Vr=V
|
||||
void McolcV ( srbvvec Vr, const srbvmat M, int c ); // Vr=M.column(c)
|
||||
void McolcMcol ( srbvmat Mr, int cr, const srbvmat M, int c ); // Vr.column(cr)=M.column(c)
|
||||
void MxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1*M2
|
||||
void MTxM ( srbvmat Mr, const srbvmat M1, const srbvmat M2 ); // Mr=M1.transpose()*M2
|
||||
void MxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 ); // Vr=M1*V1
|
||||
void MxVpV ( srbvvec Vr, const srbvmat M1, const srbvvec V1, const srbvvec V2 );
|
||||
void MTxV ( srbvvec Vr, const srbvmat M1, const srbvvec V1 );
|
||||
void VmV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 );
|
||||
void VpV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 );
|
||||
void VpVxS ( srbvvec Vr, const srbvvec V1, const srbvvec V2, srbvreal s );
|
||||
void VcrossV ( srbvvec Vr, const srbvvec V1, const srbvvec V2 );
|
||||
srbvreal VdotV ( const srbvvec V1, const srbvvec V2 );
|
||||
srbvreal VdistV2 ( const srbvvec V1, const srbvvec V2 );
|
||||
void VxS ( srbvvec Vr, const srbvvec V, srbvreal s );
|
||||
void Meigen ( srbvmat vout, srbvvec dout, srbvmat a );
|
||||
|
||||
/*! SegPoints returns closest points between a segment pair.
|
||||
Points x and y are the found closest points.
|
||||
Parameters p and a are segment 1 origin and vector.
|
||||
Parameters q and b are segment 2 origin and vector. */
|
||||
void SegPoints( srbvvec vec, srbvvec x, srbvvec y,
|
||||
const srbvvec p, const srbvvec a,
|
||||
const srbvvec q, const srbvvec b );
|
||||
|
||||
/*! TriDist computes the closest points on two triangles, and
|
||||
returns the distance between them.
|
||||
s and t are the triangles, stored tri[point][dimension].
|
||||
If the triangles are disjoint, p and q give the closest
|
||||
points of s and t respectively. However, if the triangles
|
||||
overlap, p and q are basically a random pair of points from
|
||||
the triangles, not coincident points on the intersection of
|
||||
the triangles, as might be expected. */
|
||||
srbvreal TriDist ( srbvvec p, srbvvec q,
|
||||
const srbvmat s, const srbvmat t );
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_BV_MATH_H
|
||||
471
source/dcdt/se/sr_bv_nbody.cpp
Normal file
471
source/dcdt/se/sr_bv_nbody.cpp
Normal file
@ -0,0 +1,471 @@
|
||||
#include "precompiled.h"
|
||||
# include <math.h>
|
||||
# include "sr_bv_nbody.h"
|
||||
# include "sr_model.h"
|
||||
# include "sr_box.h"
|
||||
# include "sr_mat.h"
|
||||
# include "sr_trace.h"
|
||||
|
||||
# define SR_USE_TRACE1 // function calls trace
|
||||
|
||||
//============================== SrBvNBody::EndPoint ====================================
|
||||
|
||||
/* EndPoint stores either the (min_x, min_y, min_z)
|
||||
or the (max_x, max_y, max_z) end-point of an AABB.
|
||||
Each instance of EndPoint appears simultaneously in three sorted linked
|
||||
lists, one for each dimension, as required by the sweep and prune algorithm.
|
||||
A "min" and the correspoding "max" end-point give us three intervals, one
|
||||
for each axis, which are actually the projections of the AABB on each of the
|
||||
three co-ordinate axii. */
|
||||
enum MinMax { MIN=1, MAX=2 };
|
||||
struct SrBvNBody::EndPoint
|
||||
{ char minmax; //whether it represents a "MIN" or a "MAX" end-point
|
||||
srbvreal val[3]; //the coordinates of the EndPoint.
|
||||
EndPoint* prev[3]; //for maintaining the three linked
|
||||
EndPoint* next[3]; //lists.
|
||||
AABB *aabb; //back pointer to the parent AABB.
|
||||
};
|
||||
|
||||
//============================== SrBvNBody::AABB ====================================
|
||||
|
||||
/* Class AABB is used to store information about an axis aligned bounding box.
|
||||
The AABB here has the same "radius" along all the three axii. As a result
|
||||
of this, even if the object rotates about some arbitrary point and axis,
|
||||
the radius of the enclosing AABB need not be changed.
|
||||
Computing the center and radius of the AABBs:
|
||||
Let P1, P2, ... , Pn be the position vectors of the end-points of the given
|
||||
object. Then, the center of the enclosing AABB is the centroid of the object:
|
||||
center = (P1 + P2 + ... + Pn) / n
|
||||
The radius is given by: radius = max_i (dist(Pi, center)) */
|
||||
struct SrBvNBody::AABB
|
||||
{ int id; // the id of the enclosed object
|
||||
srbvreal center[3]; // center of the AABB
|
||||
srbvreal radius; // radius of the AABB
|
||||
EndPoint *lo; // the (min_x, min_y, min_z) and
|
||||
EndPoint *hi; // (max_x, max_y, max_z) corners of the AABB. These point to
|
||||
// the corresponding nodes in the NBody linked lists.
|
||||
|
||||
AABB ( EndPoint* l, EndPoint* h )
|
||||
{ lo = l;
|
||||
lo->minmax = MIN;
|
||||
lo->aabb = this;
|
||||
hi = h;
|
||||
hi->minmax = MAX;
|
||||
hi->aabb = this;
|
||||
}
|
||||
|
||||
AABB ()
|
||||
{ lo = new EndPoint;
|
||||
lo->minmax = MIN;
|
||||
lo->aabb = this;
|
||||
hi = new EndPoint;
|
||||
hi->minmax = MAX;
|
||||
hi->aabb = this;
|
||||
}
|
||||
|
||||
~AABB ()
|
||||
{ delete lo;
|
||||
delete hi;
|
||||
}
|
||||
|
||||
/* min/max holds the min/max endpoints of the box, but considering the
|
||||
maximum radius as the size in all directions; this allows rotation
|
||||
of the models w/o the need to recompute the boxes */
|
||||
void compute_min_max ()
|
||||
{ lo->val[0] = center[0] - radius;
|
||||
lo->val[1] = center[1] - radius;
|
||||
lo->val[2] = center[2] - radius;
|
||||
hi->val[0] = center[0] + radius;
|
||||
hi->val[1] = center[1] + radius;
|
||||
hi->val[2] = center[2] + radius;
|
||||
}
|
||||
|
||||
friend bool overlaps ( AABB* obj1, AABB* obj2 )//check it the two AABBs overlap
|
||||
{
|
||||
int coord;
|
||||
for (coord=0; coord<3; coord++)
|
||||
{ if (obj1->lo->val[coord] < obj2->lo->val[coord])
|
||||
{ if (obj2->lo->val[coord] > obj1->hi->val[coord]) return false; }
|
||||
else
|
||||
{ if (obj1->lo->val[coord] > obj2->hi->val[coord]) return false; }
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
//============================== SrBvNBody ====================================
|
||||
|
||||
const int REVERSE = 1; //whether the direction of movement of the interval
|
||||
const int FORWARD = 2; //along the list is forward (FORWARD), reverse (REVERSE)
|
||||
const int NOCHANGE = 3; //or there is no movement at all (NOCHANGE).
|
||||
|
||||
SrBvNBody::SrBvNBody() //constructor.
|
||||
{
|
||||
_elist[0] = NULL;
|
||||
_elist[1] = NULL;
|
||||
_elist[2] = NULL;
|
||||
}
|
||||
|
||||
SrBvNBody::~SrBvNBody() //destructor.
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
void SrBvNBody::init()
|
||||
{
|
||||
while ( _aabb.size()>0 )
|
||||
{ delete _aabb.pop();
|
||||
}
|
||||
|
||||
_elist[0] = NULL;
|
||||
_elist[1] = NULL;
|
||||
_elist[2] = NULL;
|
||||
|
||||
overlapping_pairs.init ();
|
||||
}
|
||||
|
||||
bool SrBvNBody::insert_object ( int id, const SrModel& m )
|
||||
{
|
||||
SR_TRACE1 ( "insert_object" );
|
||||
|
||||
// Increase the dynamic array if necessary, and store the new aabb
|
||||
while ( id>=_aabb.size() ) _aabb.push()=0;
|
||||
if ( _aabb[id] ) return false; // error: id already exists, so just return
|
||||
AABB* aabb = new AABB;
|
||||
_aabb[id] = aabb;
|
||||
|
||||
// Set the id to the given value
|
||||
aabb->id = id;
|
||||
|
||||
// Get the center of the AABB, instead of the centroid as vcollide does
|
||||
SrBox box;
|
||||
m.get_bounding_box(box);
|
||||
SrPnt boxc = box.center();
|
||||
aabb->center[0] = boxc.x;
|
||||
aabb->center[1] = boxc.y;
|
||||
aabb->center[2] = boxc.z;
|
||||
|
||||
// The "radius" of the AABB is computed as the maximum distance of the AABB
|
||||
// center from any of the vertices of the object.
|
||||
float dist2, distmax=0;
|
||||
int i, mvsize=m.V.size();
|
||||
for ( i=0; i<mvsize; i++ )
|
||||
{ dist2 = ::dist2 ( boxc, m.V[i] );
|
||||
if ( dist2>distmax ) distmax=dist2;
|
||||
}
|
||||
aabb->radius = (srbvreal) sqrt(double(distmax));
|
||||
aabb->radius *= (srbvreal)1.0001; //add a 0.01% buffer.
|
||||
aabb->compute_min_max ();
|
||||
|
||||
// Now, check the overlap of this AABB with with all other AABBs and
|
||||
// add the pair to the set of overlapping pairs if reqd.
|
||||
int size = _aabb.size();
|
||||
for ( i=0; i<size; i++ )
|
||||
{ if ( _aabb[i] )
|
||||
if ( overlaps(aabb,_aabb[i]) )
|
||||
_add_pair ( aabb->id, i );
|
||||
}
|
||||
|
||||
// Now, for each of the three co-ordinates, insert the interval
|
||||
// in the correspoding list.
|
||||
int coord;
|
||||
for ( coord=0; coord<3; coord++ )
|
||||
{ //first insert the "hi" endpoint.
|
||||
if ( _elist[coord]==NULL ) // if the list is empty, insert in front.
|
||||
{ _elist[coord] = aabb->hi;
|
||||
aabb->hi->prev[coord] = aabb->hi->next[coord] = NULL;
|
||||
}
|
||||
else //otherwise insert in the correct location (the list is sorted)
|
||||
{ _list_insert ( coord, aabb->hi ); }
|
||||
|
||||
// Now, insert the "lo" endpoint. The list cannot be empty since we
|
||||
// have already inserted the "hi" endpoint
|
||||
_list_insert ( coord, aabb->lo );
|
||||
}
|
||||
|
||||
SR_TRACE1 ( "insert_object end" );
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrBvNBody::update_transformation ( int id, const SrMat& m )
|
||||
{
|
||||
SR_TRACE1 ( "update_transformation" );
|
||||
|
||||
if ( id>=_aabb.size() ) return false;
|
||||
if ( !_aabb[id] ) return false;
|
||||
|
||||
AABB *aabb = _aabb[id]; // the given object exists
|
||||
|
||||
// compute the new position of the AABB center
|
||||
SrPnt c ( aabb->center );
|
||||
srbvvec new_center;
|
||||
new_center[0] = m.get(0)*c.x + m.get(4)*c.y + m.get(8)*c.z + m.get(12);
|
||||
new_center[1] = m.get(1)*c.x + m.get(5)*c.y + m.get(9)*c.z + m.get(13);
|
||||
new_center[2] = m.get(2)*c.x + m.get(6)*c.y + m.get(10)*c.z + m.get(14);
|
||||
|
||||
// vcollide version:
|
||||
//double new_center[3]; // multiply: [trans] * [center]
|
||||
//new_center[0] = aabb->center[0] * trans[0][0] + aabb->center[1] * trans[0][1] + aabb->center[2] * trans[0][2] + trans[0][3];
|
||||
//new_center[1] = aabb->center[0] * trans[1][0] + aabb->center[1] * trans[1][1] + aabb->center[2] * trans[1][2] + trans[1][3];
|
||||
//new_center[2] = aabb->center[0] * trans[2][0] + aabb->center[1] * trans[2][1] + aabb->center[2] * trans[2][2] + trans[2][3];
|
||||
|
||||
// compute the new min and max endpoints
|
||||
// (aabb min/max values are updated later on)
|
||||
srbvreal min[3], max[3];
|
||||
min[0] = new_center[0] - aabb->radius;
|
||||
min[1] = new_center[1] - aabb->radius;
|
||||
min[2] = new_center[2] - aabb->radius;
|
||||
max[0] = new_center[0] + aabb->radius;
|
||||
max[1] = new_center[1] + aabb->radius;
|
||||
max[2] = new_center[2] + aabb->radius;
|
||||
|
||||
// we need these so that we can use the same function overlaps(AABB *, AABB *)
|
||||
// to check overlap of the newly transformed object with other objects
|
||||
EndPoint lo, hi;
|
||||
AABB dummy(&lo,&hi);
|
||||
lo.val[0]=min[0]; lo.val[1]=min[1]; lo.val[2]=min[2];
|
||||
hi.val[0]=max[0]; hi.val[1]=max[1]; hi.val[2]=max[2];
|
||||
|
||||
//update all the three lists by moving the endpoint to correct position.
|
||||
EndPoint *temp;
|
||||
int direction;
|
||||
int coord;
|
||||
for ( coord=0; coord<3; coord++ )
|
||||
{ // set the direction of motion of the endpoint along the list
|
||||
if ( aabb->lo->val[coord] > min[coord] )
|
||||
direction = REVERSE;
|
||||
else if ( aabb->lo->val[coord] < min[coord] )
|
||||
direction = FORWARD;
|
||||
else
|
||||
direction = NOCHANGE;
|
||||
|
||||
if ( direction==REVERSE ) // backward motion
|
||||
{ // first update the "lo" endpoint of the interval
|
||||
if ( aabb->lo->prev[coord]!=NULL )
|
||||
{ temp = aabb->lo;
|
||||
while ( (temp != NULL) && (temp->val[coord] > min[coord]))
|
||||
{ if (temp->minmax == MAX)
|
||||
if (overlaps(temp->aabb, &dummy))
|
||||
_add_pair(temp->aabb->id, aabb->id);
|
||||
temp = temp->prev[coord];
|
||||
}
|
||||
if (temp == NULL)
|
||||
{ aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord];
|
||||
aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord];
|
||||
aabb->lo->prev[coord] = NULL;
|
||||
aabb->lo->next[coord] = _elist[coord];
|
||||
_elist[coord]->prev[coord] = aabb->lo;
|
||||
_elist[coord] = aabb->lo;
|
||||
}
|
||||
else
|
||||
{ aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord];
|
||||
aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord];
|
||||
aabb->lo->prev[coord] = temp;
|
||||
aabb->lo->next[coord] = temp->next[coord];
|
||||
temp->next[coord]->prev[coord] = aabb->lo;
|
||||
temp->next[coord] = aabb->lo;
|
||||
}
|
||||
}
|
||||
aabb->lo->val[coord] = min[coord];
|
||||
//then update the "hi" endpoint of the interval.
|
||||
if (aabb->hi->val[coord] != max[coord])
|
||||
{ temp = aabb->hi;
|
||||
while (temp->val[coord] > max[coord])
|
||||
{ if ( (temp->minmax == MIN) && (overlaps(temp->aabb, aabb)) )
|
||||
_del_pair(temp->aabb->id, aabb->id);
|
||||
temp = temp->prev[coord];
|
||||
}
|
||||
aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord];
|
||||
if (aabb->hi->next[coord] != NULL)
|
||||
aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord];
|
||||
aabb->hi->prev[coord] = temp;
|
||||
aabb->hi->next[coord] = temp->next[coord];
|
||||
if (temp->next[coord] != NULL)
|
||||
temp->next[coord]->prev[coord] = aabb->hi;
|
||||
temp->next[coord] = aabb->hi;
|
||||
aabb->hi->val[coord] = max[coord];
|
||||
}
|
||||
}
|
||||
else if (direction == FORWARD) // forward motion
|
||||
{ // here, we first update the "hi" endpoint
|
||||
if ( aabb->hi->next[coord] != NULL )
|
||||
{ temp = aabb->hi;
|
||||
while ( (temp->next[coord] != NULL) && (temp->val[coord] < max[coord]) )
|
||||
{ if (temp->minmax == MIN)
|
||||
if (overlaps(temp->aabb, &dummy))
|
||||
_add_pair(temp->aabb->id, aabb->id);
|
||||
temp = temp->next[coord];
|
||||
}
|
||||
if (temp->val[coord] < max[coord])
|
||||
{ aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord];
|
||||
aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord];
|
||||
aabb->hi->prev[coord] = temp;
|
||||
aabb->hi->next[coord] = NULL;
|
||||
temp->next[coord] = aabb->hi;
|
||||
}
|
||||
else if (aabb->hi->val[coord] != max[coord])
|
||||
{ aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord];
|
||||
aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord];
|
||||
aabb->hi->prev[coord] = temp->prev[coord];
|
||||
aabb->hi->next[coord] = temp;
|
||||
temp->prev[coord]->next[coord] = aabb->hi;
|
||||
temp->prev[coord] = aabb->hi;
|
||||
}
|
||||
}
|
||||
aabb->hi->val[coord] = max[coord];
|
||||
//then, update the "lo" endpoint of the interval.
|
||||
temp = aabb->lo;
|
||||
while (temp->val[coord] < min[coord])
|
||||
{ if ( (temp->minmax == MAX) && (overlaps(temp->aabb, aabb)) )
|
||||
_del_pair(temp->aabb->id, aabb->id);
|
||||
temp = temp->next[coord];
|
||||
}
|
||||
if (aabb->lo->prev[coord] != NULL)
|
||||
{ aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord]; }
|
||||
else
|
||||
{ _elist[coord] = aabb->lo->next[coord]; }
|
||||
aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord];
|
||||
aabb->lo->prev[coord] = temp->prev[coord];
|
||||
aabb->lo->next[coord] = temp;
|
||||
if (temp->prev[coord] != NULL)
|
||||
{ temp->prev[coord]->next[coord] = aabb->lo; }
|
||||
else
|
||||
{ _elist[coord] = aabb->lo; }
|
||||
temp->prev[coord] = aabb->lo;
|
||||
aabb->lo->val[coord] = min[coord];
|
||||
}
|
||||
}
|
||||
|
||||
// make sure AABB destructor does not delete the used static vars:
|
||||
dummy.lo = 0;
|
||||
dummy.hi = 0;
|
||||
|
||||
SR_TRACE1 ( "update_transformation end" );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrBvNBody::remove_object ( int id )
|
||||
{
|
||||
SR_TRACE1 ( "remove_object" );
|
||||
|
||||
if ( id>=_aabb.size() ) return false;
|
||||
if ( !_aabb[id] ) return false;
|
||||
|
||||
// 1st get the AABB to be deleted
|
||||
AABB *aabb = _aabb[id];
|
||||
|
||||
// Now "remove it" from the AABB array
|
||||
if ( id+1==_aabb.size() )
|
||||
_aabb.pop();
|
||||
else
|
||||
_aabb[id] = NULL;
|
||||
|
||||
// Now we delete all the three intervals from the corresponding lists
|
||||
int coord;
|
||||
for ( coord=0; coord<3; coord++ )
|
||||
{ // first delete the "lo" endpoint of the interval
|
||||
if ( aabb->lo->prev[coord]==NULL )
|
||||
_elist[coord] = aabb->lo->next[coord];
|
||||
else
|
||||
aabb->lo->prev[coord]->next[coord] = aabb->lo->next[coord];
|
||||
aabb->lo->next[coord]->prev[coord] = aabb->lo->prev[coord];
|
||||
// then, delete the "hi" endpoint
|
||||
if ( aabb->hi->prev[coord]==NULL )
|
||||
_elist[coord] = aabb->hi->next[coord];
|
||||
else
|
||||
aabb->hi->prev[coord]->next[coord] = aabb->hi->next[coord];
|
||||
if ( aabb->hi->next[coord]!=NULL )
|
||||
aabb->hi->next[coord]->prev[coord] = aabb->hi->prev[coord];
|
||||
}
|
||||
|
||||
// Delete all entries involving this id from the set of overlapping pairs
|
||||
overlapping_pairs.del_pairs_with_id ( id );
|
||||
|
||||
// de-allocate the memory
|
||||
delete aabb;
|
||||
|
||||
SR_TRACE1 ( "remove_object end" );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//=========================== private methods ===================================
|
||||
|
||||
void SrBvNBody::_list_insert ( int coord, EndPoint* newpt )
|
||||
{
|
||||
// Find the correct location in the list and insert there (the list is sorted)
|
||||
EndPoint* current = _elist[coord];
|
||||
while ( current->next[coord] && current->val[coord]<newpt->val[coord] )
|
||||
current = current->next[coord];
|
||||
if ( current->val[coord]>=newpt->val[coord] ) // insert before
|
||||
{ newpt->prev[coord] = current->prev[coord];
|
||||
newpt->next[coord] = current;
|
||||
if ( !current->prev[coord] )
|
||||
_elist[coord] = newpt;
|
||||
else
|
||||
current->prev[coord]->next[coord] = newpt;
|
||||
current->prev[coord] = newpt;
|
||||
}
|
||||
else // insert at the end
|
||||
{ newpt->prev[coord] = current;
|
||||
newpt->next[coord] = NULL;
|
||||
current->next[coord] = newpt;
|
||||
}
|
||||
}
|
||||
|
||||
/************************************************************************\
|
||||
|
||||
Copyright 1997 The University of North Carolina at Chapel Hill.
|
||||
All Rights Reserved.
|
||||
|
||||
Permission to use, copy, modify and distribute this software
|
||||
and its documentation for educational, research and non-profit
|
||||
purposes, without fee, and without a written agreement is
|
||||
hereby granted, provided that the above copyright notice and
|
||||
the following three paragraphs appear in all copies.
|
||||
|
||||
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL
|
||||
HILL BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL,
|
||||
INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS,
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION,
|
||||
EVEN IF THE UNIVERSITY OF NORTH CAROLINA HAVE BEEN ADVISED OF
|
||||
THE POSSIBILITY OF SUCH DAMAGES.
|
||||
|
||||
|
||||
Permission to use, copy, modify and distribute this software
|
||||
and its documentation for educational, research and non-profit
|
||||
purposes, without fee, and without a written agreement is
|
||||
hereby granted, provided that the above copyright notice and
|
||||
the following three paragraphs appear in all copies.
|
||||
|
||||
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||||
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS"
|
||||
BASIS, AND THE UNIVERSITY OF NORTH CAROLINA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
|
||||
MODIFICATIONS.
|
||||
|
||||
|
||||
---------------------------------
|
||||
|Please send all BUG REPORTS to: |
|
||||
| |
|
||||
| geom@cs.unc.edu |
|
||||
| |
|
||||
---------------------------------
|
||||
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
US Mail: A. Pattekar/J. Cohen/T. Hudson/S. Gottschalk/M. Lin/D. Manocha
|
||||
Department of Computer Science
|
||||
Sitterson Hall, CB #3175
|
||||
University of N. Carolina
|
||||
Chapel Hill, NC 27599-3175
|
||||
|
||||
Phone: (919)962-1749
|
||||
|
||||
EMail: geom@cs.unc.edu
|
||||
|
||||
\************************************************************************/
|
||||
65
source/dcdt/se/sr_bv_nbody.h
Normal file
65
source/dcdt/se/sr_bv_nbody.h
Normal file
@ -0,0 +1,65 @@
|
||||
/** \file sr_bv_nbody.h
|
||||
* multibody AABB seep and prune */
|
||||
|
||||
# ifndef SR_BV_NBODY_H
|
||||
# define SR_BV_NBODY_H
|
||||
|
||||
# include "sr_bv_math.h"
|
||||
# include "sr_bv_id_pairs.h"
|
||||
|
||||
class SrModel;
|
||||
class SrMat;
|
||||
|
||||
/*! Maintains a set of three linked lists and keeps updating them using the
|
||||
sweep and prune algorithm. At any point in execution, the class maintains
|
||||
a set of pairs of objects whose AABBs overlap. In general, this set would
|
||||
be very small compared to the n(n-1)/2 possible pairs.
|
||||
Adapted from the VCollide package. Their copyright is in the source file. */
|
||||
class SrBvNBody
|
||||
{ private:
|
||||
struct AABB;
|
||||
struct EndPoint;
|
||||
EndPoint *_elist[3]; //Pointers to the three linked lists, one per axis
|
||||
SrArray<AABB*> _aabb; //Element "i" points to the AABB of the object with id=i. If
|
||||
// this object doesn't exist, then the pointer is NULL.
|
||||
public:
|
||||
/*! at any instance during execution, overlapping_pairs contains the set
|
||||
of pairs of ids whose AABBs overlap. */
|
||||
SrBvIdPairs overlapping_pairs;
|
||||
|
||||
public:
|
||||
/*! Constructor */
|
||||
SrBvNBody();
|
||||
|
||||
/*! Destructor (calls init()) */
|
||||
~SrBvNBody();
|
||||
|
||||
/*! Clears everything */
|
||||
void init ();
|
||||
|
||||
/*! Add an object with the given id to the NBody data structure.
|
||||
If the id already exists, nothing is done and false is returned.
|
||||
Otherwise, the object is added and true is returned.
|
||||
Note: to change a model associated to an existing id, delete_object()
|
||||
must be called prior to add_object() */
|
||||
bool insert_object ( int id, const SrModel& m );
|
||||
|
||||
/*! Update position and orientation of the associated object, and
|
||||
the 3 per-axis linked lists maintaining the ordered AABB endpoints.
|
||||
False is returned if the id is invalid; true is returned otherwise. */
|
||||
bool update_transformation ( int id, const SrMat& m );
|
||||
|
||||
/*! Delete the object from the data structure. Returns true if the
|
||||
object could be deleted, and false otherwise. False may only
|
||||
occur if the id is invalid or represents an already deleted object */
|
||||
bool remove_object ( int id );
|
||||
|
||||
private:
|
||||
void _list_insert ( int coord, EndPoint* newpt );
|
||||
void _add_pair(int id1, int id2)
|
||||
{ if (id1 != id2) overlapping_pairs.add_pair(id1, id2); }
|
||||
void _del_pair(int id1, int id2)
|
||||
{ if (id1 != id2) overlapping_pairs.del_pair(id1, id2); }
|
||||
};
|
||||
|
||||
#endif // SR_BV_NBODY_H
|
||||
331
source/dcdt/se/sr_bv_tree.cpp
Normal file
331
source/dcdt/se/sr_bv_tree.cpp
Normal file
@ -0,0 +1,331 @@
|
||||
#include "precompiled.h"/* Note: this code was adapted from the PQP library;
|
||||
their copyright notice can be found at the end of this file. */
|
||||
|
||||
# include "sr_bv_tree.h"
|
||||
# include "sr_model.h"
|
||||
|
||||
//====================================
|
||||
//======= SrBvTree::SrBvTree =========
|
||||
//====================================
|
||||
|
||||
SrBvTree::SrBvTree ()
|
||||
{
|
||||
_last_tri=0;
|
||||
}
|
||||
|
||||
//=====================================
|
||||
//======= SrBvTree::~SrBvTree =========
|
||||
//=====================================
|
||||
SrBvTree::~SrBvTree ()
|
||||
{
|
||||
}
|
||||
|
||||
//================================
|
||||
//======= SrBvTree::init =========
|
||||
//================================
|
||||
void SrBvTree::init ()
|
||||
{
|
||||
_tris.size(0); _tris.compress();
|
||||
_bvs.size(0); _bvs.compress();
|
||||
_last_tri=0;
|
||||
}
|
||||
|
||||
//====================================
|
||||
//======= SrBvTree::make =========
|
||||
//====================================
|
||||
void SrBvTree::make ( const SrModel& m )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
if ( m.F.size()==0 ) { init(); return; }
|
||||
|
||||
// get the geometry of the model:
|
||||
_tris.size ( m.F.size() );
|
||||
int i;
|
||||
for ( i=0; i<_tris.size(); i++ )
|
||||
{ Vset ( _tris[i].p1, m.V[m.F[i].a] );
|
||||
Vset ( _tris[i].p2, m.V[m.F[i].b] );
|
||||
Vset ( _tris[i].p3, m.V[m.F[i].c] );
|
||||
_tris[i].id = i;
|
||||
}
|
||||
_last_tri = &_tris[0];
|
||||
|
||||
// create an array of BVs for the model:
|
||||
_bvs.ensure_capacity ( 2*_tris.size()-1 );
|
||||
_bvs.size ( 1 );
|
||||
|
||||
// build recursively the tree:
|
||||
_build_recurse ( 0/*node*/, 0/*first*/, _tris.size()/*num*/ );
|
||||
|
||||
// change BV orientations from world-relative to parent-relative:
|
||||
srbvmat R; srbvvec T;
|
||||
Midentity(R);
|
||||
Videntity(T);
|
||||
_make_parent_relative(0,R,T,T);
|
||||
|
||||
// make sure the sizes were well defined:
|
||||
_tris.compress();
|
||||
_bvs.compress();
|
||||
}
|
||||
|
||||
static void get_centroid_triverts ( srbvvec c, SrBvTri *tris, int num_tris )
|
||||
{
|
||||
int i;
|
||||
|
||||
c[0] = c[1] = c[2] = 0.0;
|
||||
|
||||
// get center of mass
|
||||
for(i=0; i<num_tris; i++)
|
||||
{
|
||||
srbvreal *p1 = tris[i].p1;
|
||||
srbvreal *p2 = tris[i].p2;
|
||||
srbvreal *p3 = tris[i].p3;
|
||||
|
||||
c[0] += p1[0] + p2[0] + p3[0];
|
||||
c[1] += p1[1] + p2[1] + p3[1];
|
||||
c[2] += p1[2] + p2[2] + p3[2];
|
||||
}
|
||||
|
||||
srbvreal n = (srbvreal)(3 * num_tris);
|
||||
|
||||
c[0] /= n;
|
||||
c[1] /= n;
|
||||
c[2] /= n;
|
||||
}
|
||||
|
||||
static void get_covariance_triverts ( srbvmat M, SrBvTri *tris, int num_tris )
|
||||
{
|
||||
int i;
|
||||
srbvreal S1[3];
|
||||
srbvreal S2[3][3];
|
||||
|
||||
S1[0] = S1[1] = S1[2] = 0.0;
|
||||
S2[0][0] = S2[1][0] = S2[2][0] = 0.0;
|
||||
S2[0][1] = S2[1][1] = S2[2][1] = 0.0;
|
||||
S2[0][2] = S2[1][2] = S2[2][2] = 0.0;
|
||||
|
||||
// get center of mass
|
||||
for(i=0; i<num_tris; i++)
|
||||
{
|
||||
srbvreal *p1 = tris[i].p1;
|
||||
srbvreal *p2 = tris[i].p2;
|
||||
srbvreal *p3 = tris[i].p3;
|
||||
|
||||
S1[0] += p1[0] + p2[0] + p3[0];
|
||||
S1[1] += p1[1] + p2[1] + p3[1];
|
||||
S1[2] += p1[2] + p2[2] + p3[2];
|
||||
|
||||
S2[0][0] += (p1[0] * p1[0] +
|
||||
p2[0] * p2[0] +
|
||||
p3[0] * p3[0]);
|
||||
S2[1][1] += (p1[1] * p1[1] +
|
||||
p2[1] * p2[1] +
|
||||
p3[1] * p3[1]);
|
||||
S2[2][2] += (p1[2] * p1[2] +
|
||||
p2[2] * p2[2] +
|
||||
p3[2] * p3[2]);
|
||||
S2[0][1] += (p1[0] * p1[1] +
|
||||
p2[0] * p2[1] +
|
||||
p3[0] * p3[1]);
|
||||
S2[0][2] += (p1[0] * p1[2] +
|
||||
p2[0] * p2[2] +
|
||||
p3[0] * p3[2]);
|
||||
S2[1][2] += (p1[1] * p1[2] +
|
||||
p2[1] * p2[2] +
|
||||
p3[1] * p3[2]);
|
||||
}
|
||||
|
||||
srbvreal n = (srbvreal)(3 * num_tris);
|
||||
|
||||
// now get covariances
|
||||
|
||||
M[0][0] = S2[0][0] - S1[0]*S1[0] / n;
|
||||
M[1][1] = S2[1][1] - S1[1]*S1[1] / n;
|
||||
M[2][2] = S2[2][2] - S1[2]*S1[2] / n;
|
||||
M[0][1] = S2[0][1] - S1[0]*S1[1] / n;
|
||||
M[1][2] = S2[1][2] - S1[1]*S1[2] / n;
|
||||
M[0][2] = S2[0][2] - S1[0]*S1[2] / n;
|
||||
M[1][0] = M[0][1];
|
||||
M[2][0] = M[0][2];
|
||||
M[2][1] = M[1][2];
|
||||
}
|
||||
|
||||
// given a list of triangles, a splitting axis, and a coordinate on
|
||||
// that axis, partition the triangles into two groups according to
|
||||
// where their centroids fall on the axis (under axial projection).
|
||||
// Returns the number of tris in the first half
|
||||
static int split_tris ( SrBvTri* tris, int num_tris, srbvvec a, srbvreal c )
|
||||
{
|
||||
int i;
|
||||
int c1 = 0;
|
||||
srbvvec p;
|
||||
srbvreal x;
|
||||
SrBvTri temp;
|
||||
|
||||
using namespace SrBvMath;
|
||||
|
||||
for(i = 0; i < num_tris; i++)
|
||||
{
|
||||
// loop invariant: up to (but not including) index c1 in group 1,
|
||||
// then up to (but not including) index i in group 2
|
||||
//
|
||||
// [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
|
||||
// c1 i
|
||||
//
|
||||
VcV(p, tris[i].p1);
|
||||
VpV(p, p, tris[i].p2);
|
||||
VpV(p, p, tris[i].p3);
|
||||
x = VdotV(p, a);
|
||||
x /= 3.0;
|
||||
if (x <= c)
|
||||
{
|
||||
// group 1
|
||||
temp = tris[i];
|
||||
tris[i] = tris[c1];
|
||||
tris[c1] = temp;
|
||||
c1++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// group 2 -- do nothing
|
||||
}
|
||||
}
|
||||
|
||||
// split arbitrarily if one group empty
|
||||
|
||||
if ((c1 == 0) || (c1 == num_tris)) c1 = num_tris/2;
|
||||
|
||||
return c1;
|
||||
}
|
||||
|
||||
// Fits m->child(bn) to the num_tris triangles starting at first_tri
|
||||
// Then, if num_tris is greater than one, partitions the tris into two
|
||||
// sets, and recursively builds two children of m->child(bn)
|
||||
void SrBvTree::_build_recurse ( int bn, int first_tri, int num_tris )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
SrBv* b = child ( bn );
|
||||
|
||||
// compute a rotation matrix
|
||||
|
||||
srbvreal C[3][3], E[3][3], R[3][3], s[3], axis[3], mean[3], coord;
|
||||
|
||||
get_covariance_triverts(C,&_tris[first_tri],num_tris);
|
||||
|
||||
Meigen(E, s, C);
|
||||
|
||||
// place axes of E in order of increasing s
|
||||
|
||||
int min, mid, max;
|
||||
if (s[0] > s[1]) { max = 0; min = 1; }
|
||||
else { min = 0; max = 1; }
|
||||
if (s[2] < s[min]) { mid = min; min = 2; }
|
||||
else if (s[2] > s[max]) { mid = max; max = 2; }
|
||||
else { mid = 2; }
|
||||
McolcMcol(R,0,E,max);
|
||||
McolcMcol(R,1,E,mid);
|
||||
R[0][2] = E[1][max]*E[2][mid] - E[1][mid]*E[2][max];
|
||||
R[1][2] = E[0][mid]*E[2][max] - E[0][max]*E[2][mid];
|
||||
R[2][2] = E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
|
||||
|
||||
// fit the BV
|
||||
b->fit ( R, &_tris[first_tri], num_tris );
|
||||
|
||||
if (num_tris == 1)
|
||||
{
|
||||
// BV is a leaf BV - first_child will index a triangle
|
||||
b->first_child = -(first_tri + 1);
|
||||
}
|
||||
else if (num_tris > 1)
|
||||
{
|
||||
// BV not a leaf - first_child will index a BV
|
||||
b->first_child = _bvs.size();
|
||||
_bvs.push();
|
||||
_bvs.push();
|
||||
|
||||
// choose splitting axis and splitting coord
|
||||
McolcV(axis,R,0);
|
||||
|
||||
get_centroid_triverts(mean,&_tris[first_tri],num_tris);
|
||||
coord = VdotV(axis, mean);
|
||||
|
||||
// now split
|
||||
int num_first_half = split_tris(&_tris[first_tri], num_tris,
|
||||
axis, coord);
|
||||
|
||||
// recursively build the children
|
||||
_build_recurse( child(bn)->first_child, first_tri, num_first_half);
|
||||
_build_recurse( child(bn)->first_child + 1,
|
||||
first_tri + num_first_half, num_tris - num_first_half);
|
||||
}
|
||||
}
|
||||
|
||||
// this descends the hierarchy, converting world-relative
|
||||
// transforms to parent-relative transforms
|
||||
void SrBvTree::_make_parent_relative ( int bn,
|
||||
const srbvmat parentR,
|
||||
const srbvvec parentTr,
|
||||
const srbvvec parentTo )
|
||||
{
|
||||
srbvmat Rpc;
|
||||
srbvvec Tpc;
|
||||
|
||||
if ( !child(bn)->leaf() )
|
||||
{ // make children parent-relative
|
||||
_make_parent_relative ( child(bn)->first_child,
|
||||
child(bn)->R,
|
||||
child(bn)->Tr,
|
||||
child(bn)->To
|
||||
);
|
||||
_make_parent_relative ( child(bn)->first_child+1,
|
||||
child(bn)->R,
|
||||
child(bn)->Tr,
|
||||
child(bn)->To
|
||||
);
|
||||
}
|
||||
|
||||
// make self parent relative
|
||||
using namespace SrBvMath;
|
||||
MTxM(Rpc,parentR,child(bn)->R);
|
||||
McM(child(bn)->R,Rpc);
|
||||
VmV(Tpc,child(bn)->Tr,parentTr);
|
||||
MTxV(child(bn)->Tr,parentR,Tpc);
|
||||
VmV(Tpc,child(bn)->To,parentTo);
|
||||
MTxV(child(bn)->To,parentR,Tpc);
|
||||
}
|
||||
|
||||
/*************************************************************************\
|
||||
Copyright 1999 The University of North Carolina at Chapel Hill.
|
||||
All Rights Reserved.
|
||||
|
||||
Permission to use, copy, modify and distribute this software and its
|
||||
documentation for educational, research and non-profit purposes, without
|
||||
fee, and without a written agreement is hereby granted, provided that the
|
||||
above copyright notice and the following three paragraphs appear in all
|
||||
copies.
|
||||
|
||||
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
|
||||
LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
|
||||
CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
|
||||
USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
|
||||
OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
|
||||
DAMAGES.
|
||||
|
||||
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||||
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
|
||||
PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
|
||||
NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
|
||||
UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
US Mail: S. Gottschalk, E. Larsen
|
||||
Department of Computer Science
|
||||
Sitterson Hall, CB #3175
|
||||
University of N. Carolina
|
||||
Chapel Hill, NC 27599-3175
|
||||
Phone: (919)962-1749
|
||||
EMail: geom@cs.unc.edu
|
||||
\**************************************************************************/
|
||||
87
source/dcdt/se/sr_bv_tree.h
Normal file
87
source/dcdt/se/sr_bv_tree.h
Normal file
@ -0,0 +1,87 @@
|
||||
/** \file sr_bv_tree.h
|
||||
* bounding volume tree */
|
||||
|
||||
# ifndef SR_BV_TREE_H
|
||||
# define SR_BV_TREE_H
|
||||
|
||||
# include "sr_array.h"
|
||||
# include "sr_bv_math.h"
|
||||
# include "sr_class_manager.h"
|
||||
|
||||
class SrModel;
|
||||
class SrBvTreeQuery;
|
||||
|
||||
/*! Encapsulates the coordinates of a triangle. */
|
||||
struct SrBvTri
|
||||
{ srbvvec p1, p2, p3; // the three coordinates
|
||||
int id; // the id of the face of the original SrModel
|
||||
};
|
||||
|
||||
/*! Encapsulates a bounding volume node.
|
||||
Adapted from PQP, see the copyright notice in the source file. */
|
||||
struct SrBv
|
||||
{ srbvmat R; // orientation of RSS & OBB
|
||||
srbvvec Tr; // RSS: position of rectangle
|
||||
srbvreal l[2]; // RSS: side lengths of rectangle
|
||||
srbvreal r; // RSS: radius of sphere summed with rectangle to form RSS
|
||||
srbvvec To; // OBB: position of obb
|
||||
srbvvec d; // OBB: (half) dimensions of obb
|
||||
int first_child; // positive value is index of first_child bv
|
||||
// negative value is -(index + 1) of triangle
|
||||
SrBv() { first_child=0; }
|
||||
bool leaf() const { return first_child<0? true:false; }
|
||||
srbvreal size() const;
|
||||
void fit ( srbvmat O, SrBvTri* tris, int num_tris );
|
||||
static bool overlap ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 );
|
||||
static srbvreal distance ( srbvmat R, srbvvec T, const SrBv* b1, const SrBv* b2 );
|
||||
};
|
||||
|
||||
/*! \class SrBvTree sr_bv_tree.h
|
||||
\brief bounding volume tree
|
||||
Manages a hierarchy of bounding volumes, both OBB and RSS are
|
||||
maintained. OBBs are used for collision detection and RSSs are
|
||||
used for distance computation.
|
||||
Adapted from PQP, see the copyright notice in the source file. */
|
||||
class SrBvTree
|
||||
{ private:
|
||||
SrArray<SrBvTri> _tris;
|
||||
SrArray<SrBv> _bvs;
|
||||
SrBvTri* _last_tri; // closest tri on this model in last distance test
|
||||
friend class SrBvTreeQuery;
|
||||
|
||||
public :
|
||||
/*! Constructor */
|
||||
SrBvTree ();
|
||||
|
||||
/*! Destructor */
|
||||
~SrBvTree ();
|
||||
|
||||
/*! Returns the number of bounding volumes in the current tree */
|
||||
int nodes () const { return _bvs.size(); }
|
||||
|
||||
/*! Returns the triangle with index i */
|
||||
const SrBvTri* tri ( int i ) const { return &_tris[i]; }
|
||||
|
||||
/*! Returns a const copy of the list of triangles */
|
||||
const SrArray<SrBvTri>& tris () const { return _tris; }
|
||||
|
||||
/*! Returns the bounding volume node with index i */
|
||||
SrBv* child ( int i ) { return &_bvs[i]; }
|
||||
const SrBv* const_child ( int i ) const { return &_bvs[i]; }
|
||||
|
||||
/*! Empty the tree and frees all used memory */
|
||||
void init ();
|
||||
|
||||
/*! Constructs the tree for the given model */
|
||||
void make ( const SrModel& m );
|
||||
|
||||
private :
|
||||
void _build_recurse ( int bn, int first_tri, int num_tris );
|
||||
void _make_parent_relative ( int bn, const srbvmat parentR,
|
||||
const srbvvec parentTr,
|
||||
const srbvvec parentTo );
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_BV_TREE_H
|
||||
839
source/dcdt/se/sr_bv_tree_query.cpp
Normal file
839
source/dcdt/se/sr_bv_tree_query.cpp
Normal file
@ -0,0 +1,839 @@
|
||||
#include "precompiled.h"/* Note: this code was adapted from the PQP library;
|
||||
their copyright notice can be found at the end of this file. */
|
||||
|
||||
# include "sr_bv_tree_query.h"
|
||||
|
||||
//====================== SrBvTreeQuery ======================
|
||||
|
||||
SrBvTreeQuery::SrBvTreeQuery ()
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
void SrBvTreeQuery::init ()
|
||||
{
|
||||
_num_bv_tests = 0;
|
||||
_num_tri_tests = 0;
|
||||
_pairs.size(0);
|
||||
_pairs.compress();
|
||||
_dist = 0;
|
||||
_rel_err = _abs_err = 0;
|
||||
_closer_than_tolerance = false;
|
||||
_toler = 0;
|
||||
}
|
||||
|
||||
inline void setmv ( srbvmat m, srbvvec v, const SrMat& srm )
|
||||
{
|
||||
# define SRM(i) (srbvreal)(srm[i])
|
||||
// here we pass from column-major to line-major order:
|
||||
m[0][0]=SRM(0); m[1][0]=SRM(1); m[2][0]=SRM(2);
|
||||
m[0][1]=SRM(4); m[1][1]=SRM(5); m[2][1]=SRM(6);
|
||||
m[0][2]=SRM(8); m[1][2]=SRM(9); m[2][2]=SRM(10);
|
||||
v[0]=SRM(12); v[1]=SRM(13); v[2]=SRM(14);
|
||||
# undef SRM
|
||||
}
|
||||
|
||||
|
||||
bool SrBvTreeQuery::collide ( const SrMat& m1, const SrBvTree* t1,
|
||||
const SrMat& m2, const SrBvTree* t2 )
|
||||
{
|
||||
srbvmat R1, R2;
|
||||
srbvvec T1, T2;
|
||||
setmv ( R1, T1, m1 );
|
||||
setmv ( R2, T2, m2 );
|
||||
collide ( R1, T1, t1, R2, T2, t2, CollideFirstContact );
|
||||
return _pairs.size()>0? true:false;
|
||||
}
|
||||
|
||||
int SrBvTreeQuery::collide_all ( const SrMat& m1, const SrBvTree* t1,
|
||||
const SrMat& m2, const SrBvTree* t2 )
|
||||
{
|
||||
srbvmat R1, R2;
|
||||
srbvvec T1, T2;
|
||||
setmv ( R1, T1, m1 );
|
||||
setmv ( R2, T2, m2 );
|
||||
collide ( R1, T1, t1, R2, T2, t2, CollideAllContacts );
|
||||
return _pairs.size();
|
||||
}
|
||||
|
||||
float SrBvTreeQuery::distance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2,
|
||||
float rel_err, float abs_err )
|
||||
{
|
||||
srbvmat R1, R2;
|
||||
srbvvec T1, T2;
|
||||
setmv ( R1, T1, m1 );
|
||||
setmv ( R2, T2, m2 );
|
||||
_distance ( R1, T1, t1, R2, T2, t2, rel_err, abs_err );
|
||||
_srp1.set ( _p1 );
|
||||
_srp2.set ( _p2 );
|
||||
return distance();
|
||||
}
|
||||
|
||||
bool SrBvTreeQuery::tolerance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2,
|
||||
float tolerance )
|
||||
{
|
||||
srbvmat R1, R2;
|
||||
srbvvec T1, T2;
|
||||
setmv ( R1, T1, m1 );
|
||||
setmv ( R2, T2, m2 );
|
||||
return SrBvTreeQuery::tolerance ( R1, T1, t1, R2, T2, t2, tolerance );
|
||||
}
|
||||
|
||||
float SrBvTreeQuery::greedy_distance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2, float delta )
|
||||
{
|
||||
srbvmat R1, R2;
|
||||
srbvvec T1, T2;
|
||||
setmv ( R1, T1, m1 );
|
||||
setmv ( R2, T2, m2 );
|
||||
return SrBvTreeQuery::greedy_distance ( R1, T1, t1, R2, T2, t2, delta );
|
||||
}
|
||||
|
||||
//====================== static functions ======================
|
||||
|
||||
inline srbvreal max ( srbvreal a, srbvreal b, srbvreal c )
|
||||
{
|
||||
srbvreal t = a;
|
||||
if (b > t) t = b;
|
||||
if (c > t) t = c;
|
||||
return t;
|
||||
}
|
||||
|
||||
inline srbvreal min ( srbvreal a, srbvreal b, srbvreal c )
|
||||
{
|
||||
srbvreal t = a;
|
||||
if (b < t) t = b;
|
||||
if (c < t) t = c;
|
||||
return t;
|
||||
}
|
||||
|
||||
static int project6 ( srbvreal *ax,
|
||||
srbvreal *p1, srbvreal *p2, srbvreal *p3,
|
||||
srbvreal *q1, srbvreal *q2, srbvreal *q3 )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
srbvreal P1 = VdotV(ax, p1);
|
||||
srbvreal P2 = VdotV(ax, p2);
|
||||
srbvreal P3 = VdotV(ax, p3);
|
||||
srbvreal Q1 = VdotV(ax, q1);
|
||||
srbvreal Q2 = VdotV(ax, q2);
|
||||
srbvreal Q3 = VdotV(ax, q3);
|
||||
|
||||
srbvreal mx1 = max(P1, P2, P3);
|
||||
srbvreal mn1 = min(P1, P2, P3);
|
||||
srbvreal mx2 = max(Q1, Q2, Q3);
|
||||
srbvreal mn2 = min(Q1, Q2, Q3);
|
||||
|
||||
if (mn1 > mx2) return 0;
|
||||
if (mn2 > mx1) return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// very robust triangle intersection test
|
||||
// uses no divisions
|
||||
// works on coplanar triangles
|
||||
static int TriContact(
|
||||
const srbvreal *P1, const srbvreal *P2, const srbvreal *P3,
|
||||
const srbvreal *Q1, const srbvreal *Q2, const srbvreal *Q3 )
|
||||
{
|
||||
// One triangle is (p1,p2,p3). Other is (q1,q2,q3).
|
||||
// Edges are (e1,e2,e3) and (f1,f2,f3).
|
||||
// Normals are n1 and m1
|
||||
// Outwards are (g1,g2,g3) and (h1,h2,h3).
|
||||
//
|
||||
// We assume that the triangle vertices are in the same coordinate system.
|
||||
//
|
||||
// First thing we do is establish a new c.s. so that p1 is at (0,0,0).
|
||||
|
||||
srbvreal p1[3], p2[3], p3[3];
|
||||
srbvreal q1[3], q2[3], q3[3];
|
||||
srbvreal e1[3], e2[3], e3[3];
|
||||
srbvreal f1[3], f2[3], f3[3];
|
||||
srbvreal g1[3], g2[3], g3[3];
|
||||
srbvreal h1[3], h2[3], h3[3];
|
||||
srbvreal n1[3], m1[3];
|
||||
|
||||
srbvreal ef11[3], ef12[3], ef13[3];
|
||||
srbvreal ef21[3], ef22[3], ef23[3];
|
||||
srbvreal ef31[3], ef32[3], ef33[3];
|
||||
|
||||
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
|
||||
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
|
||||
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
|
||||
|
||||
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
|
||||
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
|
||||
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
|
||||
|
||||
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
|
||||
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
|
||||
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
|
||||
|
||||
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
|
||||
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
|
||||
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
|
||||
|
||||
using namespace SrBvMath;
|
||||
|
||||
VcrossV(n1, e1, e2);
|
||||
VcrossV(m1, f1, f2);
|
||||
|
||||
VcrossV(g1, e1, n1);
|
||||
VcrossV(g2, e2, n1);
|
||||
VcrossV(g3, e3, n1);
|
||||
VcrossV(h1, f1, m1);
|
||||
VcrossV(h2, f2, m1);
|
||||
VcrossV(h3, f3, m1);
|
||||
|
||||
VcrossV(ef11, e1, f1);
|
||||
VcrossV(ef12, e1, f2);
|
||||
VcrossV(ef13, e1, f3);
|
||||
VcrossV(ef21, e2, f1);
|
||||
VcrossV(ef22, e2, f2);
|
||||
VcrossV(ef23, e2, f3);
|
||||
VcrossV(ef31, e3, f1);
|
||||
VcrossV(ef32, e3, f2);
|
||||
VcrossV(ef33, e3, f3);
|
||||
|
||||
// now begin the series of tests
|
||||
|
||||
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
|
||||
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
|
||||
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**************************/
|
||||
/******* COLLIDE **********/
|
||||
/**************************/
|
||||
|
||||
void SrBvTreeQuery::collide (
|
||||
srbvmat R1, srbvvec T1, const SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, const SrBvTree* o2,
|
||||
CollideFlag flag )
|
||||
{
|
||||
// clear the data:
|
||||
_num_bv_tests = 0;
|
||||
_num_tri_tests = 0;
|
||||
_pairs.size(0);
|
||||
|
||||
// make sure that the models are built:
|
||||
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
|
||||
|
||||
// Okay, compute what transform [R,T] that takes us from cs1 to cs2.
|
||||
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
|
||||
// First compute the rotation part, then translation part
|
||||
using namespace SrBvMath;
|
||||
MTxM(_R,R1,R2);
|
||||
srbvvec Ttemp;
|
||||
VmV(Ttemp, T2, T1);
|
||||
MTxV(_T, R1, Ttemp);
|
||||
|
||||
// compute the transform from o1->child(0) to o2->child(0)
|
||||
srbvmat Rtemp, R;
|
||||
srbvvec T;
|
||||
MxM(Rtemp,_R,o2->const_child(0)->R);
|
||||
MTxM(R,o1->const_child(0)->R,Rtemp);
|
||||
MxVpV(Ttemp,_R,o2->const_child(0)->To,_T);
|
||||
VmV(Ttemp,Ttemp,o1->const_child(0)->To);
|
||||
MTxV(T,o1->const_child(0)->R,Ttemp);
|
||||
|
||||
// now start with both top level BVs
|
||||
_collide_recurse ( R, T, o1, 0, o2, 0, flag );
|
||||
}
|
||||
|
||||
float SrBvTreeQuery::greedy_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2, float delta )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
MTxM(_R,R1,R2);
|
||||
double Ttemp[3];
|
||||
VmV(Ttemp, T2, T1);
|
||||
MTxV(_T, R1, Ttemp);
|
||||
|
||||
_num_bv_tests = 0;
|
||||
_num_tri_tests = 0;
|
||||
|
||||
srbvmat Rtemp, R;
|
||||
srbvvec T;
|
||||
|
||||
MxM(Rtemp,_R,o2->child(0)->R);
|
||||
MTxM(R,o1->child(0)->R,Rtemp);
|
||||
|
||||
MxVpV(Ttemp,_R,o2->child(0)->Tr,_T);
|
||||
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
|
||||
MTxV(T,o1->child(0)->R,Ttemp);
|
||||
|
||||
// first, see if top-level BVs are separated
|
||||
_num_bv_tests += 1;
|
||||
srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0));
|
||||
|
||||
// if we already found separation don't bother recursing to refine it
|
||||
if ( float(d) > delta )
|
||||
return (float)d;
|
||||
else // if top-level BVs are not separated, recurse
|
||||
return (float)_greedy_distance_recurse(R,T,o1,0,o2,0,delta);
|
||||
}
|
||||
|
||||
//====================== private methods ======================
|
||||
|
||||
inline
|
||||
srbvreal
|
||||
TriDistance(srbvreal R[3][3], srbvreal T[3], const SrBvTri *t1, const SrBvTri *t2,
|
||||
srbvreal p[3], srbvreal q[3])
|
||||
{
|
||||
// transform tri 2 into same space as tri 1
|
||||
|
||||
srbvreal tri1[3][3], tri2[3][3];
|
||||
using namespace SrBvMath;
|
||||
VcV(tri1[0], t1->p1);
|
||||
VcV(tri1[1], t1->p2);
|
||||
VcV(tri1[2], t1->p3);
|
||||
MxVpV(tri2[0], R, t2->p1, T);
|
||||
MxVpV(tri2[1], R, t2->p2, T);
|
||||
MxVpV(tri2[2], R, t2->p3, T);
|
||||
|
||||
return TriDist(p,q,tri1,tri2);
|
||||
}
|
||||
|
||||
void SrBvTreeQuery::_collide_recurse (
|
||||
srbvmat R, srbvvec T, // b2 relative to b1
|
||||
const SrBvTree* o1, int b1,
|
||||
const SrBvTree* o2, int b2, int flag)
|
||||
{
|
||||
// first thing, see if we're overlapping
|
||||
|
||||
_num_bv_tests++;
|
||||
|
||||
if ( !SrBv::overlap(R,T,o1->const_child(b1), o2->const_child(b2))) return;
|
||||
|
||||
// if we are, see if we test triangles next
|
||||
|
||||
bool l1 = o1->const_child(b1)->leaf();
|
||||
bool l2 = o2->const_child(b2)->leaf();
|
||||
using namespace SrBvMath;
|
||||
|
||||
if (l1 && l2)
|
||||
{
|
||||
_num_tri_tests++;
|
||||
|
||||
// transform the points in b2 into space of b1, then compare
|
||||
const SrBvTri *t1 = o1->tri(-o1->const_child(b1)->first_child - 1);
|
||||
const SrBvTri *t2 = o2->tri(-o2->const_child(b2)->first_child - 1);
|
||||
srbvreal q1[3], q2[3], q3[3];
|
||||
const srbvreal *p1 = t1->p1;
|
||||
const srbvreal *p2 = t1->p2;
|
||||
const srbvreal *p3 = t1->p3;
|
||||
MxVpV(q1, _R, t2->p1, _T);
|
||||
MxVpV(q2, _R, t2->p2, _T);
|
||||
MxVpV(q3, _R, t2->p3, _T);
|
||||
if (TriContact(p1, p2, p3, q1, q2, q3))
|
||||
{ // add this to result
|
||||
_pairs.push() = t1->id;
|
||||
_pairs.push() = t2->id;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// we dont, so decide whose children to visit next
|
||||
|
||||
srbvreal sz1 = o1->const_child(b1)->size();
|
||||
srbvreal sz2 = o2->const_child(b2)->size();
|
||||
|
||||
srbvreal Rc[3][3],Tc[3],Ttemp[3];
|
||||
|
||||
if (l2 || (!l1 && (sz1 > sz2)))
|
||||
{
|
||||
int c1 = o1->const_child(b1)->first_child;
|
||||
int c2 = c1 + 1;
|
||||
|
||||
MTxM(Rc,o1->const_child(c1)->R,R);
|
||||
VmV(Ttemp,T,o1->const_child(c1)->To);
|
||||
MTxV(Tc,o1->const_child(c1)->R,Ttemp);
|
||||
_collide_recurse(Rc,Tc,o1,c1,o2,b2,flag);
|
||||
|
||||
if ( flag==CollideFirstContact && _pairs.size()>0 ) return;
|
||||
|
||||
MTxM(Rc,o1->const_child(c2)->R,R);
|
||||
VmV(Ttemp,T,o1->const_child(c2)->To);
|
||||
MTxV(Tc,o1->const_child(c2)->R,Ttemp);
|
||||
_collide_recurse(Rc,Tc,o1,c2,o2,b2,flag);
|
||||
}
|
||||
else
|
||||
{
|
||||
int c1 = o2->const_child(b2)->first_child;
|
||||
int c2 = c1 + 1;
|
||||
|
||||
MxM(Rc,R,o2->const_child(c1)->R);
|
||||
MxVpV(Tc,R,o2->const_child(c1)->To,T);
|
||||
_collide_recurse(Rc,Tc,o1,b1,o2,c1,flag);
|
||||
|
||||
if ( flag==CollideFirstContact && _pairs.size()>0 ) return;
|
||||
|
||||
MxM(Rc,R,o2->const_child(c2)->R);
|
||||
MxVpV(Tc,R,o2->const_child(c2)->To,T);
|
||||
_collide_recurse(Rc,Tc,o1,b1,o2,c2,flag);
|
||||
}
|
||||
}
|
||||
|
||||
/***************************/
|
||||
/******* DISTANCE **********/
|
||||
/***************************/
|
||||
void SrBvTreeQuery::_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2,
|
||||
srbvreal rel_err, srbvreal abs_err )
|
||||
{
|
||||
// init data:
|
||||
_dist = 0;
|
||||
_num_bv_tests = 0;
|
||||
_num_tri_tests = 0;
|
||||
_abs_err = abs_err;
|
||||
_rel_err = rel_err;
|
||||
|
||||
// make sure that the models are built:
|
||||
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
|
||||
|
||||
// Okay, compute what transform [R,T] that takes us from cs2 to cs1.
|
||||
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
|
||||
// First compute the rotation part, then translation part
|
||||
using namespace SrBvMath;
|
||||
|
||||
MTxM(_R,R1,R2);
|
||||
srbvreal Ttemp[3];
|
||||
VmV(Ttemp, T2, T1);
|
||||
MTxV(_T, R1, Ttemp);
|
||||
|
||||
// establish initial upper bound using last triangles which
|
||||
// provided the minimum distance
|
||||
srbvvec p,q;
|
||||
_dist = TriDistance(_R,_T,o1->_last_tri,o2->_last_tri,p,q);
|
||||
VcV(_p1,p);
|
||||
VcV(_p2,q);
|
||||
|
||||
// compute the transform from o1->child(0) to o2->child(0)
|
||||
srbvreal Rtemp[3][3], R[3][3], T[3];
|
||||
|
||||
MxM(Rtemp,_R,o2->const_child(0)->R);
|
||||
MTxM(R,o1->const_child(0)->R,Rtemp);
|
||||
|
||||
MxVpV(Ttemp,_R,o2->const_child(0)->Tr,_T);
|
||||
VmV(Ttemp,Ttemp,o1->const_child(0)->Tr);
|
||||
MTxV(T,o1->const_child(0)->R,Ttemp);
|
||||
|
||||
_distance_recurse(R,T,o1,0,o2,0);
|
||||
|
||||
// res->p2 is in cs 1 ; transform it to cs 2
|
||||
srbvvec u;
|
||||
VmV(u, _p2, _T);
|
||||
MTxV(_p2, _R, u);
|
||||
}
|
||||
|
||||
void SrBvTreeQuery::_distance_recurse (
|
||||
srbvmat R, srbvvec T, // b2 relative to b1
|
||||
SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2 )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
srbvreal sz1 = o1->const_child(b1)->size();
|
||||
srbvreal sz2 = o2->const_child(b2)->size();
|
||||
bool l1 = o1->const_child(b1)->leaf();
|
||||
bool l2 = o2->const_child(b2)->leaf();
|
||||
|
||||
if ( l1 && l2 )
|
||||
{ // both leaves. Test the triangles beneath them.
|
||||
_num_tri_tests++;
|
||||
|
||||
srbvreal p[3], q[3];
|
||||
|
||||
SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1];
|
||||
SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1];
|
||||
|
||||
srbvreal d = TriDistance(_R,_T,t1,t2,p,q);
|
||||
|
||||
if ( d<_dist )
|
||||
{
|
||||
_dist = d;
|
||||
|
||||
VcV(_p1, p); // p already in c.s. 1
|
||||
VcV(_p2, q); // q must be transformed
|
||||
// into c.s. 2 later
|
||||
o1->_last_tri = t1;
|
||||
o2->_last_tri = t2;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// First, perform distance tests on the children. Then traverse
|
||||
// them recursively, but test the closer pair first, the further
|
||||
// pair second.
|
||||
|
||||
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
|
||||
srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
|
||||
|
||||
if (l2 || (!l1 && (sz1 > sz2)))
|
||||
{
|
||||
// visit the children of b1
|
||||
|
||||
a1 = o1->child(b1)->first_child;
|
||||
a2 = b2;
|
||||
c1 = o1->child(b1)->first_child+1;
|
||||
c2 = b2;
|
||||
|
||||
MTxM(R1,o1->const_child(a1)->R,R);
|
||||
VmV(Ttemp,T,o1->const_child(a1)->Tr);
|
||||
MTxV(T1,o1->const_child(a1)->R,Ttemp);
|
||||
|
||||
MTxM(R2,o1->const_child(c1)->R,R);
|
||||
VmV(Ttemp,T,o1->const_child(c1)->Tr);
|
||||
MTxV(T2,o1->const_child(c1)->R,Ttemp);
|
||||
}
|
||||
else
|
||||
{
|
||||
// visit the children of b2
|
||||
a1 = b1;
|
||||
a2 = o2->child(b2)->first_child;
|
||||
c1 = b1;
|
||||
c2 = o2->child(b2)->first_child+1;
|
||||
|
||||
MxM(R1,R,o2->const_child(a2)->R);
|
||||
MxVpV(T1,R,o2->const_child(a2)->Tr,T);
|
||||
|
||||
MxM(R2,R,o2->const_child(c2)->R);
|
||||
MxVpV(T2,R,o2->const_child(c2)->Tr,T);
|
||||
}
|
||||
|
||||
_num_bv_tests += 2;
|
||||
|
||||
srbvreal d1 = SrBv::distance(R1, T1, o1->const_child(a1), o2->const_child(a2));
|
||||
srbvreal d2 = SrBv::distance(R2, T2, o1->const_child(c1), o2->const_child(c2));
|
||||
|
||||
if (d2 < d1)
|
||||
{
|
||||
if ((d2 < (_dist - _abs_err)) ||
|
||||
(d2*(1 + _rel_err) < _dist))
|
||||
{
|
||||
_distance_recurse(R2, T2, o1, c1, o2, c2);
|
||||
}
|
||||
|
||||
if ((d1 < (_dist - _abs_err)) ||
|
||||
(d1*(1 + _rel_err) < _dist))
|
||||
{
|
||||
_distance_recurse(R1, T1, o1, a1, o2, a2);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((d1 < (_dist - _abs_err)) ||
|
||||
(d1*(1 + _rel_err) < _dist))
|
||||
{
|
||||
_distance_recurse(R1, T1, o1, a1, o2, a2);
|
||||
}
|
||||
|
||||
if ((d2 < (_dist - _abs_err)) ||
|
||||
(d2*(1 + _rel_err) < _dist))
|
||||
{
|
||||
_distance_recurse(R2, T2, o1, c1, o2, c2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/***************************/
|
||||
/******* TOLERANCE *********/
|
||||
/***************************/
|
||||
|
||||
void SrBvTreeQuery::_tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2,
|
||||
srbvreal tolerance )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
// init data:
|
||||
// _dist = 0;
|
||||
_num_bv_tests = 0;
|
||||
_num_tri_tests = 0;
|
||||
if ( tolerance<0 ) tolerance=0.0;
|
||||
_toler = tolerance;
|
||||
_closer_than_tolerance = false;
|
||||
|
||||
// make sure that the models are built:
|
||||
if ( o1->nodes()==0 || o2->nodes()==0 ) return;
|
||||
|
||||
// Compute the transform [R,T] that takes us from cs2 to cs1.
|
||||
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
|
||||
|
||||
MTxM(_R,R1,R2);
|
||||
srbvreal Ttemp[3];
|
||||
VmV(Ttemp, T2, T1);
|
||||
MTxV(_T, R1, Ttemp);
|
||||
|
||||
// compute the transform from o1->child(0) to o2->child(0)
|
||||
srbvreal Rtemp[3][3], R[3][3], T[3];
|
||||
|
||||
MxM(Rtemp,_R,o2->child(0)->R);
|
||||
MTxM(R,o1->child(0)->R,Rtemp);
|
||||
MxVpV(Ttemp,_R,o2->child(0)->Tr,_T);
|
||||
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
|
||||
MTxV(T,o1->child(0)->R,Ttemp);
|
||||
|
||||
// find a distance lower bound for trivial reject
|
||||
|
||||
srbvreal d = SrBv::distance(R, T, o1->child(0), o2->child(0));
|
||||
|
||||
if ( d <= _toler )
|
||||
{ _tolerance_recurse ( R, T, o1, 0, o2, 0 );
|
||||
}
|
||||
|
||||
// res->p2 is in cs 1 ; transform it to cs 2
|
||||
srbvreal u[3];
|
||||
VmV(u, _p2, _T);
|
||||
MTxV(_p2, _R, u);
|
||||
}
|
||||
|
||||
void SrBvTreeQuery::_tolerance_recurse
|
||||
( srbvmat R, srbvvec T,
|
||||
SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2 )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
srbvreal sz1 = o1->child(b1)->size();
|
||||
srbvreal sz2 = o2->child(b2)->size();
|
||||
int l1 = o1->child(b1)->leaf();
|
||||
int l2 = o2->child(b2)->leaf();
|
||||
|
||||
if (l1 && l2)
|
||||
{
|
||||
// both leaves - find if tri pair within tolerance
|
||||
|
||||
_num_tri_tests++;
|
||||
|
||||
srbvreal p[3], q[3];
|
||||
|
||||
SrBvTri *t1 = &o1->_tris[-o1->child(b1)->first_child - 1];
|
||||
SrBvTri *t2 = &o2->_tris[-o2->child(b2)->first_child - 1];
|
||||
|
||||
srbvreal d = TriDistance(_R,_T,t1,t2,p,q);
|
||||
|
||||
if ( d<=_toler )
|
||||
{
|
||||
// triangle pair distance less than tolerance
|
||||
_closer_than_tolerance = true;
|
||||
_dist = d;
|
||||
VcV(_p1, p); // p already in c.s. 1
|
||||
VcV(_p2, q); // q must be transformed into c.s. 2 later
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
|
||||
srbvreal R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
|
||||
|
||||
if (l2 || (!l1 && (sz1 > sz2)))
|
||||
{
|
||||
// visit the children of b1
|
||||
|
||||
a1 = o1->child(b1)->first_child;
|
||||
a2 = b2;
|
||||
c1 = o1->child(b1)->first_child+1;
|
||||
c2 = b2;
|
||||
|
||||
MTxM(R1,o1->child(a1)->R,R);
|
||||
VmV(Ttemp,T,o1->child(a1)->Tr);
|
||||
MTxV(T1,o1->child(a1)->R,Ttemp);
|
||||
|
||||
MTxM(R2,o1->child(c1)->R,R);
|
||||
VmV(Ttemp,T,o1->child(c1)->Tr);
|
||||
MTxV(T2,o1->child(c1)->R,Ttemp);
|
||||
}
|
||||
else
|
||||
{
|
||||
// visit the children of b2
|
||||
|
||||
a1 = b1;
|
||||
a2 = o2->child(b2)->first_child;
|
||||
c1 = b1;
|
||||
c2 = o2->child(b2)->first_child+1;
|
||||
|
||||
MxM(R1,R,o2->child(a2)->R);
|
||||
MxVpV(T1,R,o2->child(a2)->Tr,T);
|
||||
MxM(R2,R,o2->child(c2)->R);
|
||||
MxVpV(T2,R,o2->child(c2)->Tr,T);
|
||||
}
|
||||
|
||||
_num_bv_tests += 2;
|
||||
|
||||
srbvreal d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2));
|
||||
srbvreal d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2));
|
||||
|
||||
if (d2 < d1)
|
||||
{
|
||||
if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2);
|
||||
if (_closer_than_tolerance) return;
|
||||
if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d1 <= _toler) _tolerance_recurse( R1, T1, o1, a1, o2, a2);
|
||||
if (_closer_than_tolerance) return;
|
||||
if (d2 <= _toler) _tolerance_recurse( R2, T2, o1, c1, o2, c2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
srbvreal SrBvTreeQuery::_greedy_distance_recurse (
|
||||
srbvmat R, srbvvec T, SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2, srbvreal delta )
|
||||
{
|
||||
using namespace SrBvMath;
|
||||
|
||||
int l1 = o1->child(b1)->leaf();
|
||||
int l2 = o2->child(b2)->leaf();
|
||||
|
||||
if (l1 && l2)
|
||||
{
|
||||
// both leaves. return their distance
|
||||
_num_tri_tests++;
|
||||
|
||||
double p[3], q[3];
|
||||
|
||||
const SrBvTri *t1 = o1->tri(-o1->child(b1)->first_child - 1);
|
||||
const SrBvTri *t2 = o2->tri(-o2->child(b2)->first_child - 1);
|
||||
|
||||
return TriDistance(_R,_T,t1,t2,p,q);
|
||||
}
|
||||
|
||||
// First, perform distance tests on the children. Then traverse
|
||||
// them recursively, but test the closer pair first, the further
|
||||
// pair second.
|
||||
|
||||
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
|
||||
double R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
|
||||
|
||||
srbvreal sz1 = o1->child(b1)->size();
|
||||
srbvreal sz2 = o2->child(b2)->size();
|
||||
|
||||
if (l2 || (!l1 && (sz1 > sz2)))
|
||||
{
|
||||
// visit the children of b1
|
||||
|
||||
a1 = o1->child(b1)->first_child;
|
||||
a2 = b2;
|
||||
c1 = o1->child(b1)->first_child+1;
|
||||
c2 = b2;
|
||||
|
||||
MTxM(R1,o1->child(a1)->R,R);
|
||||
VmV(Ttemp,T,o1->child(a1)->Tr);
|
||||
MTxV(T1,o1->child(a1)->R,Ttemp);
|
||||
|
||||
MTxM(R2,o1->child(c1)->R,R);
|
||||
VmV(Ttemp,T,o1->child(c1)->Tr);
|
||||
MTxV(T2,o1->child(c1)->R,Ttemp);
|
||||
}
|
||||
else
|
||||
{
|
||||
// visit the children of b2
|
||||
|
||||
a1 = b1;
|
||||
a2 = o2->child(b2)->first_child;
|
||||
c1 = b1;
|
||||
c2 = o2->child(b2)->first_child+1;
|
||||
|
||||
MxM(R1,R,o2->child(a2)->R);
|
||||
MxVpV(T1,R,o2->child(a2)->Tr,T);
|
||||
|
||||
MxM(R2,R,o2->child(c2)->R);
|
||||
MxVpV(T2,R,o2->child(c2)->Tr,T);
|
||||
}
|
||||
|
||||
double d1 = SrBv::distance(R1, T1, o1->child(a1), o2->child(a2));
|
||||
double d2 = SrBv::distance(R2, T2, o1->child(c1), o2->child(c2));
|
||||
_num_bv_tests += 2;
|
||||
|
||||
// if we already found separation, don't further recurse to refine it
|
||||
double min_d1d2 = (d1 < d2) ? d1 : d2;
|
||||
if ( min_d1d2 > delta )
|
||||
return min_d1d2;
|
||||
|
||||
// else recurse
|
||||
if (d2 < d1) // and thus at least d2 <= delta (d1 maybe too)
|
||||
{
|
||||
srbvreal alpha = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta);
|
||||
|
||||
if ( alpha > delta ) {
|
||||
if ( d1 > delta )
|
||||
return (alpha < d1)? alpha: d1;
|
||||
srbvreal beta = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta);
|
||||
if ( beta > delta )
|
||||
return (alpha < beta)? alpha: beta;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
else // at least d1 <= delta (d2 maybe too)
|
||||
{
|
||||
srbvreal alpha = _greedy_distance_recurse(R1, T1, o1, a1, o2, a2, delta);
|
||||
|
||||
if ( alpha > delta ) {
|
||||
if ( d2 > delta )
|
||||
return (alpha < d2)? alpha: d2;
|
||||
srbvreal beta = _greedy_distance_recurse(R2, T2, o1, c1, o2, c2, delta);
|
||||
if ( beta > delta )
|
||||
return (alpha < beta)? alpha: beta;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*************************************************************************\
|
||||
Copyright 1999 The University of North Carolina at Chapel Hill.
|
||||
All Rights Reserved.
|
||||
|
||||
Permission to use, copy, modify and distribute this software and its
|
||||
documentation for educational, research and non-profit purposes, without
|
||||
fee, and without a written agreement is hereby granted, provided that the
|
||||
above copyright notice and the following three paragraphs appear in all
|
||||
copies.
|
||||
|
||||
IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
|
||||
LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
|
||||
CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
|
||||
USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
|
||||
OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
|
||||
DAMAGES.
|
||||
|
||||
THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
|
||||
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
|
||||
PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
|
||||
NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
|
||||
UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
The authors may be contacted via:
|
||||
|
||||
US Mail: S. Gottschalk, E. Larsen
|
||||
Department of Computer Science
|
||||
Sitterson Hall, CB #3175
|
||||
University of N. Carolina
|
||||
Chapel Hill, NC 27599-3175
|
||||
Phone: (919)962-1749
|
||||
EMail: geom@cs.unc.edu
|
||||
\**************************************************************************/
|
||||
173
source/dcdt/se/sr_bv_tree_query.h
Normal file
173
source/dcdt/se/sr_bv_tree_query.h
Normal file
@ -0,0 +1,173 @@
|
||||
/** \file sr_bv_tree_query.h
|
||||
* bounding volume tree */
|
||||
|
||||
// TO TEST:
|
||||
// 1. use of double vs. float
|
||||
// 2. use inline in SrBvMath functions
|
||||
|
||||
# ifndef SR_BV_TREE_QUERY_H
|
||||
# define SR_BV_TREE_QUERY_H
|
||||
|
||||
# include "sr_mat.h"
|
||||
# include "sr_bv_tree.h"
|
||||
|
||||
/*! Encapsulates data for collision and proximity queries between
|
||||
trees of bounding volumes.
|
||||
Adapted from PQP, see the copyright notice in the source file. */
|
||||
class SrBvTreeQuery
|
||||
{ private :
|
||||
// common data for all tests:
|
||||
int _num_bv_tests;
|
||||
int _num_tri_tests;
|
||||
srbvmat _R; // xform from model 1 to model 2
|
||||
srbvvec _T;
|
||||
// for collision queries only:
|
||||
SrArray<int> _pairs;
|
||||
// for both distance and tolerance queries:
|
||||
srbvreal _dist;
|
||||
SrPnt _srp1, _srp2;
|
||||
srbvvec _p1, _p2;
|
||||
// for distance queries only:
|
||||
srbvreal _rel_err;
|
||||
srbvreal _abs_err;
|
||||
// for proximity queries only:
|
||||
bool _closer_than_tolerance;
|
||||
srbvreal _toler;
|
||||
|
||||
public :
|
||||
/*! Constructor */
|
||||
SrBvTreeQuery ();
|
||||
|
||||
/*! Initialize all values and frees all used memory */
|
||||
void init ();
|
||||
|
||||
/*! Returns the number of bounding volume tests in last query */
|
||||
int num_bv_tests() const { return _num_bv_tests; }
|
||||
|
||||
/*! Returns the number of triangle intersection tests in last query */
|
||||
int num_tri_tests() const { return _num_tri_tests; }
|
||||
|
||||
/*! Returns the array with the indices of the colliding faces in
|
||||
the last collide() query. The indices pairs are sequentially
|
||||
stored in the array. */
|
||||
const SrArray<int>& colliding_pairs () const { return _pairs; }
|
||||
|
||||
/*! Returns the points computed in the last proximity or distance
|
||||
query. For a proximity query, these are the points that
|
||||
stablished the distance smaller than the tolerance (otherwise
|
||||
these points are not meaningful).
|
||||
For a distance query, these points stablished the minimum
|
||||
distance, within the relative and absolute error bounds specified.*/
|
||||
const SrPnt& p1 () const { return _srp1; }
|
||||
const SrPnt& p2 () const { return _srp2; }
|
||||
|
||||
/*! Returns the distance computed in the last proximity or distance
|
||||
query. For a proximity query, this value is only meaningful
|
||||
if the distance is smaller than the tolerance specified */
|
||||
float distance() const { return (float)_dist; }
|
||||
|
||||
/*! The boolean says whether the models in the last proximity query
|
||||
are closer than tolerance distance specified */
|
||||
bool closer_than_tolerance() const { return _closer_than_tolerance; }
|
||||
|
||||
/*! Find collisions between two models given as bv trees.
|
||||
m1 is the placement of model 1 in the world &
|
||||
m2 is the placement of model 2 in the world.
|
||||
Matrices are in column-major order (as OpenGL) and must specify
|
||||
only a rigid transformation (rotation and/or translation).
|
||||
In this version, the collision test stops when the first
|
||||
contact is found; which can be retrieved with colliding_pairs();
|
||||
True is returned if a collision was found, and false otherwise. */
|
||||
bool collide ( const SrMat& m1, const SrBvTree* t1,
|
||||
const SrMat& m2, const SrBvTree* t2 );
|
||||
|
||||
/*! Similar to collide(), but it finds all collisions between the
|
||||
two given models. Two times the number of collisions is
|
||||
returned, ie, the size of the colliding_pairs() array. */
|
||||
int collide_all ( const SrMat& m1, const SrBvTree* t1,
|
||||
const SrMat& m2, const SrBvTree* t2 );
|
||||
|
||||
/*! Computes the distance between two models given as bv trees.
|
||||
"rel_err" is the relative error margin from actual distance.
|
||||
"abs_err" is the absolute error margin from actual distance.
|
||||
The smaller of the two will be satisfied, so set one large
|
||||
to nullify its effect.
|
||||
Returns the distance between the two models.
|
||||
Methods distance(), p1() and p2() will return the distance and
|
||||
points computed during the query.
|
||||
Note: pointers t1 and t2 are not const only because the _last_tri
|
||||
member variable of SrBvTree is updated during the query. */
|
||||
float distance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2,
|
||||
float rel_err, float abs_err );
|
||||
|
||||
/*! Checks if distance between two models (given as bv trees) is <= tolerance
|
||||
The algorithm returns whether the true distance is <= or >
|
||||
"tolerance". This routine does not simply compute true distance
|
||||
and compare to the tolerance - models can often be shown closer or
|
||||
farther than the tolerance more trivially. In most cases this
|
||||
query should run faster than a distance query would on the same
|
||||
models and configurations.
|
||||
The returned parameter returns the result of the query, which can
|
||||
be also retrieved with method closer_than_tolerance().
|
||||
If the models are closer than ( <= ) tolerance, the points that
|
||||
established this can be retrieved with methods p1(), and p2(),
|
||||
and the distance can be retrieved with method distance() */
|
||||
bool tolerance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2, float tolerance );
|
||||
|
||||
float greedy_distance ( const SrMat& m1, SrBvTree* t1,
|
||||
const SrMat& m2, SrBvTree* t2, float delta );
|
||||
|
||||
public : // lower level methods
|
||||
|
||||
enum CollideFlag { CollideAllContacts=1, CollideFirstContact=2 };
|
||||
|
||||
/*! Lower level call used by the two main collide methods.
|
||||
Collision results, if any, are stored in colliding_pairs(). */
|
||||
void collide ( srbvmat R1, srbvvec T1, const SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, const SrBvTree* o2, CollideFlag flag );
|
||||
|
||||
/*! Lower level call called by the other tolerance() method */
|
||||
bool tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2, float toler )
|
||||
{ _tolerance ( R1, T1, o1, R2, T2, o2, toler );
|
||||
_srp1.set ( _p1 );
|
||||
_srp2.set ( _p2 );
|
||||
return _closer_than_tolerance;
|
||||
}
|
||||
|
||||
/*! Computes a simple but fast lower bound on the distance between two models */
|
||||
float greedy_distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2, float delta );
|
||||
|
||||
private :
|
||||
|
||||
void _collide_recurse ( srbvmat R, srbvvec T,
|
||||
const SrBvTree* o1, int b1,
|
||||
const SrBvTree* o2, int b2, int flag );
|
||||
|
||||
void _distance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2,
|
||||
srbvreal rel_err, srbvreal abs_err );
|
||||
|
||||
void _distance_recurse ( srbvmat R, srbvvec T,
|
||||
SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2 );
|
||||
|
||||
void _tolerance ( srbvmat R1, srbvvec T1, SrBvTree* o1,
|
||||
srbvmat R2, srbvvec T2, SrBvTree* o2,
|
||||
srbvreal tolerance );
|
||||
|
||||
void _tolerance_recurse ( srbvmat R, srbvvec T,
|
||||
SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2 );
|
||||
|
||||
srbvreal _greedy_distance_recurse ( srbvmat R, srbvvec T,
|
||||
SrBvTree* o1, int b1,
|
||||
SrBvTree* o2, int b2, srbvreal delta );
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_BV_TREE_QUERY_H
|
||||
202
source/dcdt/se/sr_camera.cpp
Normal file
202
source/dcdt/se/sr_camera.cpp
Normal file
@ -0,0 +1,202 @@
|
||||
#include "precompiled.h"
|
||||
# include <math.h>
|
||||
# include "sr_box.h"
|
||||
# include "sr_plane.h"
|
||||
# include "sr_camera.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // ray
|
||||
# include "sr_trace.h"
|
||||
|
||||
//=================================== SrCamera ===================================
|
||||
|
||||
SrCamera::SrCamera ()
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
SrCamera::SrCamera ( const SrCamera &c )
|
||||
:eye(c.eye), center(c.center), up(c.up)
|
||||
{
|
||||
fovy = c.fovy;
|
||||
znear = c.znear;
|
||||
zfar = c.zfar;
|
||||
aspect = c.aspect;
|
||||
scale = c.scale;
|
||||
}
|
||||
|
||||
SrCamera::SrCamera ( const SrPnt& e, const SrPnt& c, const SrVec& u )
|
||||
: eye(e), center(c), up(u)
|
||||
{
|
||||
fovy = SR_TORAD(60);
|
||||
znear = 0.1f;
|
||||
zfar = 1000.0f;
|
||||
aspect = 1.0f;
|
||||
}
|
||||
|
||||
void SrCamera::init ()
|
||||
{
|
||||
eye.set ( 0, 0, 2.0f );
|
||||
center = SrVec::null;
|
||||
up = SrVec::j;
|
||||
fovy = SR_TORAD(60);
|
||||
znear = 0.1f;
|
||||
zfar = 1000.0f;
|
||||
aspect = 1.0f;
|
||||
scale = 1.0f;
|
||||
}
|
||||
|
||||
SrMat& SrCamera::get_view_mat ( SrMat &m ) const
|
||||
{
|
||||
m.look_at ( eye, center, up );
|
||||
return m;
|
||||
}
|
||||
|
||||
SrMat& SrCamera::get_perspective_mat ( SrMat &m ) const
|
||||
{
|
||||
m.perspective ( fovy, aspect, znear, zfar );
|
||||
return m;
|
||||
}
|
||||
|
||||
// screenpt coords range in [-1,1]
|
||||
void SrCamera::get_ray ( float winx, float winy, SrVec &p1, SrVec &p2 ) const
|
||||
{
|
||||
p1.set ( winx, winy, znear ); // p1 is in the near clip plane
|
||||
|
||||
SrMat M(SrMat::NotInitialized), V(SrMat::NotInitialized), P(SrMat::NotInitialized);
|
||||
|
||||
V.look_at ( eye, center, up );
|
||||
P.perspective ( fovy, aspect, znear, zfar );
|
||||
|
||||
M.mult ( V, P ); // equiv to M = V * P
|
||||
|
||||
M.invert();
|
||||
|
||||
p1 = p1 * M;
|
||||
p2 = p1-eye; // ray is in object coordinates, but before the scaling
|
||||
|
||||
p2.normalize();
|
||||
p2 *= (zfar-znear);
|
||||
p2 += p1;
|
||||
|
||||
float inv_scale = 1.0f/scale;
|
||||
p1*= inv_scale;
|
||||
p2*= inv_scale;
|
||||
|
||||
SR_TRACE1 ( "Ray: "<< p1 <<" : "<< p2 );
|
||||
}
|
||||
|
||||
/* - -------- \
|
||||
| | | \
|
||||
h | bbox |--------------.eye
|
||||
| | | dist /
|
||||
- -------- / tan(viewang/2)=(h/2)/dist
|
||||
*/
|
||||
void SrCamera::view_all ( const SrBox &box, float fovy_radians )
|
||||
{
|
||||
SrVec size = box.size();
|
||||
float h = SR_MAX(size.x,size.y);
|
||||
|
||||
fovy = fovy_radians;
|
||||
up = SrVec::j;
|
||||
center = box.center();
|
||||
eye = center;
|
||||
|
||||
float dist = (h/2)/tanf(fovy/2);
|
||||
eye.z = box.b.z + dist;
|
||||
|
||||
float delta = box.max_size() + 0.0001f;
|
||||
zfar = SR_ABS(eye.z)+delta;
|
||||
|
||||
scale = 1.0f;
|
||||
}
|
||||
|
||||
void SrCamera::apply_translation_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy )
|
||||
{
|
||||
SrVec p1, p2, x, inc;
|
||||
|
||||
SrPlane plane ( center, eye-center );
|
||||
|
||||
get_ray ( lwinx, lwiny, p1, x );
|
||||
p1 = plane.intersect ( p1, x );
|
||||
get_ray ( winx, winy, p2, x );
|
||||
p2 = plane.intersect ( p2, x );
|
||||
|
||||
inc = p1-p2;
|
||||
|
||||
inc *= scale;
|
||||
|
||||
*this += inc;
|
||||
}
|
||||
|
||||
void SrCamera::operator*= ( const SrQuat& q )
|
||||
{
|
||||
eye -= center;
|
||||
eye = eye * q;
|
||||
eye += center;
|
||||
up -= center;
|
||||
up = up * q;
|
||||
up += center;
|
||||
}
|
||||
|
||||
void SrCamera::operator+= ( const SrVec& v )
|
||||
{
|
||||
eye += v;
|
||||
center += v;
|
||||
}
|
||||
|
||||
void SrCamera::operator-= ( const SrVec& v )
|
||||
{
|
||||
eye -= v;
|
||||
center -= v;
|
||||
}
|
||||
|
||||
//=============================== friends ==========================================
|
||||
|
||||
SrCamera operator* ( const SrCamera& c, const SrQuat& q )
|
||||
{
|
||||
SrCamera cam(c);
|
||||
cam *= q;
|
||||
return cam;
|
||||
}
|
||||
|
||||
SrCamera operator+ ( const SrCamera& c, const SrVec& v )
|
||||
{
|
||||
SrCamera cam(c);
|
||||
cam += v;
|
||||
return cam;
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& out, const SrCamera& c )
|
||||
{
|
||||
// out << "eye:" << c.eye << " center:" << c.center << " up:" << c.up << srnl;
|
||||
|
||||
out << "eye " << c.eye << srnl <<
|
||||
"center " << c.center << srnl <<
|
||||
"up " << c.up << srnl <<
|
||||
"fovy " << c.fovy << srnl <<
|
||||
"znear " << c.znear << srnl <<
|
||||
"zfar " << c.zfar << srnl <<
|
||||
"aspect " << c.aspect << srnl <<
|
||||
"scale " << c.scale << srnl;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& inp, SrCamera& c )
|
||||
{
|
||||
while ( 1 )
|
||||
{ if ( inp.get_token()==SrInput::EndOfFile ) break;
|
||||
if ( inp.last_token()=="eye" ) inp>>c.eye;
|
||||
else if ( inp.last_token()=="center" ) inp>>c.center;
|
||||
else if ( inp.last_token()=="up" ) inp>>c.up;
|
||||
else if ( inp.last_token()=="fovy" ) inp>>c.fovy;
|
||||
else if ( inp.last_token()=="znear" ) inp>>c.znear;
|
||||
else if ( inp.last_token()=="zfar" ) inp>>c.zfar;
|
||||
else if ( inp.last_token()=="aspect" ) inp>>c.aspect;
|
||||
else if ( inp.last_token()=="scale" ) inp>>c.scale;
|
||||
else { inp.unget_token(); break; }
|
||||
}
|
||||
return inp;
|
||||
}
|
||||
|
||||
//================================ End of File =========================================
|
||||
108
source/dcdt/se/sr_camera.h
Normal file
108
source/dcdt/se/sr_camera.h
Normal file
@ -0,0 +1,108 @@
|
||||
|
||||
# ifndef SR_CAMERA_H
|
||||
# define SR_CAMERA_H
|
||||
|
||||
/** \file sr_camera.h
|
||||
* Keeps camera parameters
|
||||
*/
|
||||
|
||||
# include "sr_vec.h"
|
||||
# include "sr_quat.h"
|
||||
|
||||
class SrMat;
|
||||
class SrBox;
|
||||
|
||||
/*! \class SrCamera sr_camera.h
|
||||
\brief Keeps camera parameters
|
||||
|
||||
SrCamera contains the parameters to define a camera.
|
||||
Attention: if znear is too small inconsistencies in the rendering may appear;
|
||||
a minimal value of 0.1 should be considered. */
|
||||
class SrCamera
|
||||
{ public :
|
||||
SrPnt eye; //!< position of the eye, default is (0,0,2).
|
||||
SrPnt center; //!< position where the eye is looking to, default is (0,0,0).
|
||||
SrVec up; //!< the up vector orients the camera around the eye-center vector, default is (0,1,0)
|
||||
float fovy; //!< the y field of view in radians. Default is pi/3 (60deg), range is [0.01,pi].
|
||||
float znear; //!< must be >0, default is 0.1.
|
||||
float zfar; //!< must be >0, default is 1000.
|
||||
float aspect; //!< normally is set to the screen width/heigh, default is 1.
|
||||
float scale; //!< a scale factor to be applied between the view matrix and the scene
|
||||
|
||||
public :
|
||||
|
||||
/*! Initialize the camera with the default parameters, see init(). */
|
||||
SrCamera ();
|
||||
|
||||
/*! Copy constructor. */
|
||||
SrCamera ( const SrCamera &c );
|
||||
|
||||
/*! Initialize the camera with the main parameters eye, center and up. */
|
||||
SrCamera ( const SrPnt& e, const SrPnt& c, const SrVec& u );
|
||||
|
||||
/*! Set the parameters to their default values, which are :
|
||||
eye=(0,0,2), center=(0,0,0), up=(0,1,0), fovy=60, znear=0.1, zfar=1000, aspect=1. */
|
||||
void init ();
|
||||
|
||||
/*! Set m to be the transformation matrix generated by the parameters
|
||||
eye, center, and up. A reference to m is also returned.
|
||||
Note: the scale factor is not included in this matrix. */
|
||||
SrMat& get_view_mat ( SrMat &m ) const;
|
||||
|
||||
/*! Set m to be the transformation projection matrix generated by the parameters
|
||||
fovy, znear, zfar, aspect. A reference to m is also returned. */
|
||||
SrMat& get_perspective_mat ( SrMat &m ) const;
|
||||
|
||||
/*! Gets the 3d ray (p1,p2) which projects exactly in the given window point
|
||||
according to the camera current parameters. Points p1 and p2 lye in the
|
||||
near and far planes respectively. Window points are considered to be
|
||||
in normalized coordinates, ranging between [-1,1]. */
|
||||
void get_ray ( float winx, float winy, SrVec& p1, SrVec& p2 ) const;
|
||||
|
||||
/*! Sets center at the center of the box, and put the eye in the semi-line
|
||||
rooted at center and with direction z, in a distance from the center
|
||||
that is sufficient to visualize all the box with the given fov_y parm.
|
||||
After this call, variable SrCamera::fovy will have the same value as fov_y.
|
||||
Note: the scale factor is set to one in this method. */
|
||||
void view_all ( const SrBox& box, float fov_y );
|
||||
|
||||
/*! Apply a trackball translation induced from the mouse motion. First, the ray
|
||||
passing through each window position and the intersection point with the
|
||||
projection plane is determined. Then, the two intersection points determine the
|
||||
displacement to be applied to the trackball, after a multiplication with the
|
||||
current spin rotation. Mouse coordinates must be normalized in [-1,1]x[-1,1]. */
|
||||
void apply_translation_from_mouse_motion ( float lwinx, float lwiny, float winx, float winy );
|
||||
|
||||
/*! Transforms the camera position with the given rotation. The rotation is
|
||||
applied to the up vector, and to the eye vector in the following way by
|
||||
going: x-=center; x=x*q; x+=center (x represents eye or up vector). */
|
||||
void operator*= ( const SrQuat& q );
|
||||
|
||||
/*! Adds the vector v to the eye and center points. */
|
||||
void operator+= ( const SrVec& v );
|
||||
|
||||
/*! Subtracts the vector v to the eye and center points. */
|
||||
void operator-= ( const SrVec& v );
|
||||
|
||||
/*! Returns a camera that is the same as the given camera c, but with the
|
||||
rotation q applied. See the operator *= for a description of how the
|
||||
rotation is applied to the camera. */
|
||||
friend SrCamera operator* ( const SrCamera& c, const SrQuat& q );
|
||||
|
||||
/*! Returns a camera that is the same as the given camera c, but with the
|
||||
the translation vector v added to the eye and center points. */
|
||||
friend SrCamera operator+ ( const SrCamera& c, const SrVec& v );
|
||||
|
||||
/*! Output camera data values in format keyword1 value \n keyword2 value ...
|
||||
(keywords are: eye, center, up, etc*/
|
||||
friend SrOutput& operator<< ( SrOutput& out, const SrCamera& c );
|
||||
|
||||
/*! Input camera data. Not all keywords are required to exist. The routine
|
||||
returns when a non-keyword entry is found (which is 'ungetted' in inp). */
|
||||
friend SrInput& operator>> ( SrInput& inp, SrCamera& c );
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_CAMERA_H
|
||||
|
||||
34
source/dcdt/se/sr_cfg_manager.cpp
Normal file
34
source/dcdt/se/sr_cfg_manager.cpp
Normal file
@ -0,0 +1,34 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cfg_manager.h"
|
||||
|
||||
//=========================== SrCfgManagerBase ========================================
|
||||
|
||||
// ps: could try each "level check" in a new thread
|
||||
bool SrCfgManagerBase::visible ( const srcfg* ct0, const srcfg* ct1, srcfg* ct, float prec )
|
||||
{
|
||||
float segs, dseg, dt, t;
|
||||
float len = dist ( ct0, ct1 );
|
||||
int k = 1; // k is the current level being tested
|
||||
|
||||
while ( true )
|
||||
{ // test the 2^(k-1) tests of level k:
|
||||
segs = (float) sr_pow ( 2, k );
|
||||
dseg = 1.0f / segs;
|
||||
|
||||
dt = dseg*2.0f;
|
||||
|
||||
//sr_out<<"level="<<k<<srnl;
|
||||
//sr_out<<"segs="<<segs<<srnl;
|
||||
|
||||
for ( t=dseg; t<1.0f; t+=dt )
|
||||
{ interp ( ct0, ct1, t, ct );
|
||||
//sr_out<<"t="<<t<<srnl;
|
||||
if ( !valid(ct) ) return false;
|
||||
}
|
||||
//sr_out<<"len/segs="<<(len/segs)<<" prec="<<prec<<srnl;
|
||||
if ( len/segs<=prec ) return true; // safe edge up to prec
|
||||
k++; // increment level
|
||||
}
|
||||
}
|
||||
|
||||
//============================= End of File ===========================================
|
||||
148
source/dcdt/se/sr_cfg_manager.h
Normal file
148
source/dcdt/se/sr_cfg_manager.h
Normal file
@ -0,0 +1,148 @@
|
||||
|
||||
# ifndef SR_CFG_MANAGER_H
|
||||
# define SR_CFG_MANAGER_H
|
||||
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
# include "sr_class_manager.h"
|
||||
|
||||
typedef void srcfg;
|
||||
|
||||
class SrCfgManagerBase : protected SrClassManagerBase
|
||||
{
|
||||
public : // accesible members from SrSharedClass
|
||||
|
||||
/*! Returns the current reference counter value. */
|
||||
int getref () const { return SrSharedClass::getref(); }
|
||||
|
||||
/*! Increments the reference counter. */
|
||||
void ref () { SrSharedClass::ref(); }
|
||||
|
||||
/*! Decrements the reference counter (if >0), and if the
|
||||
counter becomes 0, the class is automatically self deleted. */
|
||||
void unref() { SrSharedClass::unref(); }
|
||||
|
||||
public : // virtual callbacks
|
||||
|
||||
/*! Returns a new configuration */
|
||||
virtual srcfg* alloc ()=0;
|
||||
|
||||
/*! Returns a new configuration as a copy of c */
|
||||
virtual srcfg* alloc ( const srcfg* c )=0;
|
||||
|
||||
/*! Deletes a configuration */
|
||||
virtual void free ( srcfg* c )=0;
|
||||
|
||||
/*! Copy the contents of c2 into c1. Note: c1 and c2 will be equal
|
||||
in several calls, so a test if(c1==c2) should be included here. */
|
||||
virtual void copy ( srcfg* c1, const srcfg* c2 )=0;
|
||||
|
||||
/*! Outputs a configuration */
|
||||
virtual void output ( SrOutput& o, const srcfg* c )=0;
|
||||
|
||||
/*! Inputs a configuration */
|
||||
virtual void input ( SrInput& i, srcfg* c )=0;
|
||||
|
||||
/*! Returns a random configuration, not necessarily valid. */
|
||||
virtual void random ( srcfg* c )=0;
|
||||
|
||||
/*! Returns if the configuration is valid. */
|
||||
virtual bool valid ( const srcfg* c )=0;
|
||||
|
||||
/*! Returns a distance between the two configurations. */
|
||||
virtual float dist ( const srcfg* c1, const srcfg* c2 )=0;
|
||||
|
||||
/*! Returns the interpolated configuration c between c1 and c2,
|
||||
according to t in [0,1]. */
|
||||
virtual void interp ( const srcfg* c1, const srcfg* c2, float t, srcfg* c )=0;
|
||||
|
||||
/*! Returns true if all nodes between the interpolation of ct0 and ct1 are
|
||||
valid according to the given precision prec. Configuration ct is to be
|
||||
used as a temporary variable. Note: ct0 and ct1 are already valid.
|
||||
The default implementation for method visible performs recursive binary
|
||||
subdivision untill reaching precision prec. */
|
||||
virtual bool visible ( const srcfg* ct0, const srcfg* ct1, srcfg* ct, float prec );
|
||||
|
||||
/*! Returns true if the time encoded in c1 is smaller than in c2. This will be
|
||||
only relevant to planning in time-varying conditions, if this is not the
|
||||
case, true must always be returned */
|
||||
virtual bool monotone ( const srcfg* c1, const srcfg* c2 )=0;
|
||||
|
||||
/*! This is called just to notify that node child has been added
|
||||
as a child of node parent */
|
||||
virtual void child_added ( srcfg* parent, srcfg* child )=0;
|
||||
|
||||
/*! The time() method returns the time associated with the given configuration c.
|
||||
This method is only called when solving a time-varying problem */
|
||||
virtual float time ( srcfg* c )=0;
|
||||
};
|
||||
|
||||
/*! Template SrCfgManager makes automatic type casts to user-defined classes.
|
||||
If needed in special cases, it can be further derived to rewrite/extend
|
||||
method SrCfgManagerBase::visible().
|
||||
Here is an example of implementations of a configuration class and a class manager that
|
||||
can be directly used with template SrCfgManager<MyCfg,MyManager> : \code
|
||||
class MyCfg
|
||||
{ public :
|
||||
MyCfg ();
|
||||
MyCfg ( const MyCfg& c );
|
||||
~MyCfg ();
|
||||
void operator = ( const MyCfg& c );
|
||||
void random ();
|
||||
bool valid () const;
|
||||
friend SrOutput& operator<< ( SrOutput& out, const MyCfg& c );
|
||||
friend SrInput& operator>> ( SrInput& inp, MyCfg& c );
|
||||
friend float dist ( const MyCfg& c1, const MyCfg& c2 );
|
||||
friend float interp ( const MyCfg& c1, const MyCfg& c2, float t, MyCfg& c );
|
||||
};
|
||||
|
||||
class MyManager
|
||||
{ public :
|
||||
MyCfg* alloc () { return new MyCfg; }
|
||||
MyCfg* alloc ( const MyCfg* c ) { return new MyCfg(*c); }
|
||||
void free ( MyCfg* c ) { delete c; }
|
||||
void copy ( MyCfg* c1, const MyCfg* c2 ) { *c1=*c2; }
|
||||
void output ( SrOutput& o, const MyCfg* c ) { o<<(*c); }
|
||||
void input ( SrInput& i, MyCfg* c ) { i>>(*c); }
|
||||
void random ( MyCfg* c ) { c->random(); }
|
||||
bool valid ( const MyCfg* c ) { return c->valid(); }
|
||||
float dist ( const MyCfg* c1, const MyCfg* c2 ) { return ::dist(*c1,*c2); }
|
||||
void interp ( const MyCfg* c1, const MyCfg* c2, float t, MyCfg* c )
|
||||
{ return ::interp(*c1,*c2,t,*c); }
|
||||
void child_added ( MyCfg* parent, MyCfg* child ) {}
|
||||
}; \endcode*/
|
||||
template <class C, class M=SrCfgManagerBase>
|
||||
class SrCfgManager : public SrCfgManagerBase, public M
|
||||
{ public :
|
||||
virtual srcfg* alloc ()
|
||||
{ return (srcfg*) M::alloc(); }
|
||||
virtual srcfg* alloc ( const srcfg* c )
|
||||
{ return (srcfg*) M::alloc((const C*)c); }
|
||||
virtual void free ( srcfg* c )
|
||||
{ M::free ( (C*)c ); }
|
||||
virtual void copy ( srcfg* c1, const srcfg* c2 )
|
||||
{ M::copy ( (C*)c1, (const C*)c2 ); }
|
||||
virtual void output ( SrOutput& o, const srcfg* c )
|
||||
{ M::output ( o, (const C*)c ); }
|
||||
virtual void input ( SrInput& i, srcfg* c )
|
||||
{ M::input ( i, (C*)c ); }
|
||||
virtual void random ( srcfg* c )
|
||||
{ M::random ( (C*)c ); }
|
||||
virtual bool valid ( const srcfg* c )
|
||||
{ return M::valid ( (const C*)c ); }
|
||||
virtual float dist ( const srcfg* c1, const srcfg* c2 )
|
||||
{ return M::dist ( (const C*)c1, (const C*)c2 ); }
|
||||
virtual void interp ( const srcfg* c1, const srcfg* c2, float t, srcfg* c )
|
||||
{ M::interp ( (const C*)c1, (const C*)c2, t, (C*)c ); }
|
||||
virtual bool monotone ( const srcfg* c1, const srcfg* c2 )
|
||||
{ return M::monotone ( (const C*)c1, (const C*)c2 ); }
|
||||
virtual void child_added ( srcfg* child, srcfg* parent )
|
||||
{ M::child_added ( (C*)parent, (C*)child ); }
|
||||
virtual float time ( srcfg* c )
|
||||
{ return M::time ( (C*)c ); }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_CFG_MANAGER_H
|
||||
|
||||
442
source/dcdt/se/sr_cfg_path.cpp
Normal file
442
source/dcdt/se/sr_cfg_path.cpp
Normal file
@ -0,0 +1,442 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cfg_path.h"
|
||||
# include "sr_random.h"
|
||||
|
||||
//# define SR_USE_TRACE1 //
|
||||
# include "sr_trace.h"
|
||||
|
||||
//================================ SrCfgPathBase ========================================
|
||||
|
||||
SrCfgPathBase::SrCfgPathBase ( SrCfgManagerBase* cman )
|
||||
{
|
||||
_cman = cman;
|
||||
_cman->ref();
|
||||
_size = 0;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
SrCfgPathBase::SrCfgPathBase ( const SrCfgPathBase& p )
|
||||
{
|
||||
_cman = p._cman;
|
||||
_cman->ref();
|
||||
_size = 0;
|
||||
insert_path ( 0, p );
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
SrCfgPathBase::~SrCfgPathBase ()
|
||||
{
|
||||
init ();
|
||||
compress ();
|
||||
_cman->unref();
|
||||
}
|
||||
|
||||
void SrCfgPathBase::init ()
|
||||
{
|
||||
_size = 0;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::compress ()
|
||||
{
|
||||
while ( _buffer.size()>_size ) _cman->free ( _buffer.pop() );
|
||||
}
|
||||
|
||||
void SrCfgPathBase::push ( const srcfg* c )
|
||||
{
|
||||
if ( _buffer.size()==_size ) // add new buffer entry
|
||||
{ _buffer.push() = _cman->alloc();
|
||||
}
|
||||
if ( c ) _cman->copy ( _buffer[_size], c );
|
||||
_size++;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::pop ()
|
||||
{
|
||||
if ( _size>0 ) _size--;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::remove ( int i )
|
||||
{
|
||||
srcfg* cfg = _buffer[i];
|
||||
_buffer.move ( i/*dest*/, i+1/*src*/, _size-(i+1)/*n*/ );
|
||||
_size--;
|
||||
_buffer[_size] = cfg;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::remove ( int i, int dp )
|
||||
{
|
||||
int bsize = _buffer.size();
|
||||
int idp = i+dp;
|
||||
_buffer.size ( bsize+dp );
|
||||
_buffer.move ( bsize/*dest*/, i/*src*/, dp/*n*/ ); // copy part to remove to the buffer end
|
||||
_buffer.move ( i/*dest*/, idp/*src*/, _size-idp/*n*/ ); // remove
|
||||
_buffer.move ( _size-dp/*dest*/, bsize/*src*/, dp/*n*/ ); // keep the removed part
|
||||
_buffer.size ( bsize );
|
||||
_size -= dp;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::insert ( int i, const srcfg* c )
|
||||
{
|
||||
if ( i>=_size ) { push(c); return; }
|
||||
push ( c );
|
||||
srcfg* newcfg = top();
|
||||
_buffer.move ( i+1/*dest*/, i/*src*/, _size-(i+1)/*n*/ );
|
||||
_buffer[i] = newcfg;
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::insert_path ( int i, const SrCfgPathBase& p )
|
||||
{
|
||||
if ( p.size()<1 ) return;
|
||||
if ( i>_size ) i=_size;
|
||||
|
||||
// open space in the end:
|
||||
int oldsize = _size;
|
||||
int newsize = _size+p.size();
|
||||
while ( _size!=newsize ) push(0);
|
||||
|
||||
// move new space to the middle:
|
||||
int n, n2=newsize-(oldsize-i);
|
||||
srcfg* tmp;
|
||||
for ( n=i; n<oldsize; n++ )
|
||||
{ SR_SWAP(_buffer[n],_buffer[n2]);
|
||||
}
|
||||
|
||||
// copy contents:
|
||||
for ( n=0; n<p.size(); n++ )
|
||||
{ _cman->copy ( _buffer[i+n], p._buffer[n] );
|
||||
}
|
||||
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::append_path ( SrCfgPathBase& p )
|
||||
{
|
||||
// open space here:
|
||||
_buffer.insert ( _size, p.size() );
|
||||
|
||||
// transfer nodes from p:
|
||||
int i;
|
||||
for ( i=0; i<p.size(); i++ ) _buffer[_size+i] = p._buffer[i];
|
||||
_size += p.size();
|
||||
|
||||
// close space there:
|
||||
p._buffer.remove ( 0, p.size() );
|
||||
p._size=0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::swap ( int i, int j )
|
||||
{
|
||||
srcfg* tmp;
|
||||
SR_SWAP(_buffer[i],_buffer[j]);
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::revert ()
|
||||
{
|
||||
int i;
|
||||
int end = _size-1;
|
||||
int mid = _size/2;
|
||||
for ( i=0; i<mid; i++ )
|
||||
{ swap ( i, end );
|
||||
end--;
|
||||
}
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::size ( int s )
|
||||
{
|
||||
while ( size()<s ) push(0);
|
||||
while ( size()>s ) pop();
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
}
|
||||
|
||||
float SrCfgPathBase::len ( int i1, int i2 ) const
|
||||
{
|
||||
float l=0;
|
||||
int i;
|
||||
if ( _size<2 ) return l;
|
||||
for ( i=i1; i<i2; i++ ) l += _cman->dist(_buffer[i],_buffer[i+1]);
|
||||
return l;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::interp ( float t, srcfg* c )
|
||||
{
|
||||
if ( _buffer.size()<2 ) { _interp_start=0; _interp_startdist=0; return; }
|
||||
|
||||
// parameters _interp_start and _interp_startdist are used to optimize
|
||||
// the time search during sequential play, here we check if they can
|
||||
// be used or if we should recount the distance form the first node
|
||||
if ( t<_interp_startdist )
|
||||
{ _interp_start=0; _interp_startdist=0; }
|
||||
|
||||
float dt;
|
||||
float d = _interp_startdist;
|
||||
int i;
|
||||
|
||||
for ( i=_interp_start+1; i<_buffer.size(); i++ )
|
||||
{ dt = _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) );
|
||||
if ( d+dt>=t ) break;
|
||||
d += dt;
|
||||
}
|
||||
|
||||
if ( i==_buffer.size() ) // may happen because of imprecisions
|
||||
{ i--; t=1; }
|
||||
else
|
||||
{ t -= d; t /= dt; if ( t>1 ) t=1; }
|
||||
|
||||
_interp_start = i-1;
|
||||
_interp_startdist = d;
|
||||
|
||||
_cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, c );
|
||||
}
|
||||
|
||||
void SrCfgPathBase::temporal_interp ( float t, srcfg* c )
|
||||
{
|
||||
if ( _buffer.size()<2 ) { _interp_start=0; _interp_startdist=0; return; }
|
||||
|
||||
// here _interp_startdist is in fact the "start time"
|
||||
if ( t<_interp_startdist )
|
||||
{ _interp_start=0; _interp_startdist=0; }
|
||||
|
||||
float dt; // delta time
|
||||
float nt; // next time
|
||||
float ct = _interp_startdist; // current time
|
||||
int i;
|
||||
|
||||
for ( i=_interp_start+1; i<_buffer.size(); i++ )
|
||||
{ nt = _cman->time ( _buffer.const_get(i) );
|
||||
dt = nt-ct;
|
||||
if ( nt>=t ) break;
|
||||
ct = nt;
|
||||
}
|
||||
|
||||
if ( i==_buffer.size() ) // may happen because of imprecisions
|
||||
{ i--; t=1; }
|
||||
else
|
||||
{ t -= ct; t /= dt; if ( t>1 ) t=1; }
|
||||
|
||||
_interp_start = i-1;
|
||||
_interp_startdist = ct;
|
||||
|
||||
_cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, c );
|
||||
}
|
||||
|
||||
void SrCfgPathBase::smooth_random ( float prec, float& len )
|
||||
{
|
||||
if ( len<0 ) len = SrCfgPathBase::len(0,_size-1);
|
||||
float t1 = SrRandom::randf()*len;
|
||||
float t2 = SrRandom::randf()*len;
|
||||
linearize ( prec, len, t1, t2 );
|
||||
}
|
||||
|
||||
int SrCfgPathBase::linearize ( float prec, float& len, float t1, float t2 )
|
||||
{
|
||||
srcfg* cfg1; // 1st random cfg along the path
|
||||
srcfg* cfg2; // 2nd random cfg along the path
|
||||
srcfg* citp; // cfg used during interpolation
|
||||
|
||||
// get buffer space for cfg1 and cfg2:
|
||||
if ( _buffer.size()<_size+3 ) // add new buffer entries
|
||||
{ push(0); push(0); push(0); pop(); pop(); pop(); }
|
||||
citp = _buffer[_size];
|
||||
cfg1 = _buffer[_size+1];
|
||||
cfg2 = _buffer[_size+2];
|
||||
|
||||
// ensure t1<t2:
|
||||
float tmp;
|
||||
if ( t1>len ) t1=len;
|
||||
if ( t2>len ) t2=len;
|
||||
if ( t1>t2 ) SR_SWAP(t1,t2);
|
||||
|
||||
// get configurations cfg1 and cfg2 at t1 and t2:
|
||||
_interp_start = 0;
|
||||
_interp_startdist = 0;
|
||||
interp ( t1, cfg1 );
|
||||
int i1 = _interp_start+1; // node after t1
|
||||
interp ( t2, cfg2 );
|
||||
int i2 = _interp_start; // node prior t2
|
||||
|
||||
if ( i1>i2 ) return 0; // samples t1 and t2 are in the same edge
|
||||
|
||||
if ( !_cman->visible(cfg1,cfg2,citp,prec) ) return -1; // cannot smooth
|
||||
|
||||
len -= SrCfgPathBase::len(i1-1,i2+1); // remove the older part length from len
|
||||
|
||||
if ( i1==i2 ) // only 1 vertex between cfg1 and cfg2: insert 1 space
|
||||
{ insert ( i1+1, 0 );
|
||||
i2 = i1+1;
|
||||
}
|
||||
|
||||
// positions i1 and i2 become cfg1 and cfg2:
|
||||
_cman->copy ( _buffer[i1], cfg1 );
|
||||
_cman->copy ( _buffer[i2], cfg2 );
|
||||
|
||||
// delete non-used intermediate vertices:
|
||||
remove ( i1+1, (i2-i1)-1 );
|
||||
len += SrCfgPathBase::len(i1-1,i1+2); // add the new part length to len
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::smooth_ends ( float prec )
|
||||
{
|
||||
if ( _size<5 ) return; // not possible if <5
|
||||
|
||||
srcfg* citp; // cfg used during interpolation
|
||||
|
||||
// get buffer space for citp:
|
||||
if ( _buffer.size()<_size+1 ) { push(0); pop(); }
|
||||
citp = _buffer[_size];
|
||||
|
||||
// specify mid node and border nodes i1 and i2:
|
||||
int mid = _size/2;
|
||||
int i1 = mid;
|
||||
int i2 = mid;
|
||||
int posmax = _size-1;
|
||||
int max = _size-3;
|
||||
int min = 2;
|
||||
|
||||
// check second half:
|
||||
while ( i2<=max )
|
||||
{ if ( _cman->visible(_buffer[i2],_buffer[posmax],citp,prec) )
|
||||
{ remove ( i2+1, (posmax-i2)-1 );
|
||||
break;
|
||||
}
|
||||
i2++;
|
||||
}
|
||||
|
||||
// check first half:
|
||||
while ( i1>=min )
|
||||
{ if ( _cman->visible(_buffer[0],_buffer[i1],citp,prec) )
|
||||
{ remove ( 1, i1-1 );
|
||||
break;
|
||||
}
|
||||
i1--;
|
||||
}
|
||||
}
|
||||
|
||||
float SrCfgPathBase::_diff ( int i, float prec )
|
||||
{
|
||||
if ( i<=0 || i>=size()-1 ) return -1; // protection
|
||||
/* srcfg* cfg1;
|
||||
srcfg* cfg2;
|
||||
|
||||
// get buffer space for cfg1 and cfg2:
|
||||
if ( _buffer.size()<_size+2 ) // add new buffer entries
|
||||
{ push(0); push(0); pop(); pop(); }
|
||||
cfg1 = _buffer[_size];
|
||||
cfg2 = _buffer[_size+1];
|
||||
*/
|
||||
|
||||
float d1 = _cman->dist(_buffer.const_get(i-1),_buffer.const_get(i));
|
||||
float d2 = _cman->dist(_buffer.const_get(i),_buffer.const_get(i+1));
|
||||
float d = _cman->dist(_buffer.const_get(i-1),_buffer.const_get(i+1));
|
||||
|
||||
float diff = (d1+d2)-d;
|
||||
diff /= d;
|
||||
/*
|
||||
prec*=5.0f;
|
||||
|
||||
d = _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) );
|
||||
t = prec/d;
|
||||
if ( t>1 ) t=1;
|
||||
_cman->interp ( _buffer.const_get(i-1), _buffer.const_get(i), t, cfg1 );
|
||||
|
||||
d = _cman->dist ( _buffer.const_get(i), _buffer.const_get(i+1) );
|
||||
t = prec/d;
|
||||
if ( t>1 ) t=1;
|
||||
_cman->interp ( _buffer.const_get(i), _buffer.const_get(i+1), t, cfg2 );
|
||||
|
||||
float diff = (_cman->dist(cfg1,_buffer.const_get(i))+_cman->dist(_buffer.const_get(i),cfg2))
|
||||
-_cman->dist(cfg1,cfg2);*/
|
||||
return diff;
|
||||
}
|
||||
|
||||
void SrCfgPathBase::smooth_init ( float prec )
|
||||
{
|
||||
_sprec = prec;
|
||||
//smooth_ends ( prec );
|
||||
_slen = len();
|
||||
_sbads=0;
|
||||
_slastangmax=9999999999.0f;
|
||||
}
|
||||
|
||||
bool SrCfgPathBase::smooth_step ()
|
||||
{
|
||||
smooth_random ( _sprec, _slen );
|
||||
return false;
|
||||
int i, imax=1;
|
||||
int lasti = size()-2;
|
||||
float d;
|
||||
float angmax=0;
|
||||
float t=0, tmax=0;
|
||||
for ( i=1; i<=lasti; i++ )
|
||||
{ t += _cman->dist ( _buffer.const_get(i-1), _buffer.const_get(i) );
|
||||
d = _diff ( i, _sprec );
|
||||
if ( d>angmax )
|
||||
{ angmax=d; imax=i; tmax=t; }
|
||||
}
|
||||
|
||||
//sr_out<<"angmax => "<<angmax<<srnl;
|
||||
//sr_out<<"prec => "<<_sprec<<srnl;
|
||||
|
||||
float maxradius = _slen/10.0f;
|
||||
float r, t1, t2;
|
||||
int result;
|
||||
int times=0;
|
||||
r = maxradius;
|
||||
while ( r>_sprec )
|
||||
{ //r = SrRandom::randf()*maxradius;
|
||||
r/=2.0f;
|
||||
//sr_out<<times<<": "<<r<<srnl;
|
||||
t1 = tmax-r; if ( t1<0 ) t1=0;
|
||||
t2 = tmax+r; if ( t2>_slen ) t2=_slen;
|
||||
result = linearize ( _sprec, _slen, t1, t2 );
|
||||
times++;
|
||||
if ( result==0 ) break;
|
||||
}
|
||||
|
||||
if ( angmax>_slastangmax )
|
||||
{ _sbads++;
|
||||
}
|
||||
else
|
||||
{ _sbads=0;
|
||||
_slastangmax=angmax;
|
||||
}
|
||||
|
||||
//sr_out<<"BADS: "<<_bads<<srnl;
|
||||
//float freedom = z.freedom / float(times);
|
||||
|
||||
if ( _sbads>6 ) return true; // no more easy improvements
|
||||
return false;
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrCfgPathBase& p )
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<p.size(); i++ )
|
||||
{ o<<"path node "<<i<<":\n";
|
||||
p._cman->output ( o, p._buffer[i] );
|
||||
o<<srnl;
|
||||
}
|
||||
return o;
|
||||
}
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
148
source/dcdt/se/sr_cfg_path.h
Normal file
148
source/dcdt/se/sr_cfg_path.h
Normal file
@ -0,0 +1,148 @@
|
||||
|
||||
# ifndef SR_CFG_PATH_H
|
||||
# define SR_CFG_PATH_H
|
||||
|
||||
# include "sr_output.h"
|
||||
# include "sr_array.h"
|
||||
//# include "sr_cfg_manager.h"
|
||||
# include "sr_cfg_manager.h"
|
||||
|
||||
/*! SrCfgPathBase manages a sequence of configurations, describing
|
||||
a path in configuration space. */
|
||||
class SrCfgPathBase
|
||||
{ private :
|
||||
SrArray<srcfg*> _buffer;
|
||||
SrCfgManagerBase* _cman;
|
||||
int _size;
|
||||
int _interp_start;
|
||||
float _interp_startdist;
|
||||
float _sprec, _slen;
|
||||
int _sbads;
|
||||
float _slastangmax;
|
||||
|
||||
public :
|
||||
/*! Constructor requires a pointer to the configuration manager */
|
||||
SrCfgPathBase ( SrCfgManagerBase* cman );
|
||||
|
||||
/*! Copy constructor sharing the configuration manager */
|
||||
SrCfgPathBase ( const SrCfgPathBase& p );
|
||||
|
||||
/*! Destructor */
|
||||
~SrCfgPathBase ();
|
||||
|
||||
/*! Makes the path become empty */
|
||||
void init ();
|
||||
|
||||
/*! Deletes all non-used internal buffers */
|
||||
void compress ();
|
||||
|
||||
/*! Appends a configuration at the end of the path.
|
||||
Even if c is null, a new valid configuration is pushed. */
|
||||
void push ( const srcfg* c );
|
||||
|
||||
/*! Removes the last configuration in the path */
|
||||
void pop ();
|
||||
|
||||
/*! Removes the position i, which must be a valid position */
|
||||
void remove ( int i );
|
||||
|
||||
/*! Removes dp positions at position i (parameters must be valid) */
|
||||
void remove ( int i, int dp );
|
||||
|
||||
/*! Inserts a configuraton at position i; if i>=size() a push() is done. */
|
||||
void insert ( int i, const srcfg* c );
|
||||
|
||||
/*! Inserts a copy of path p at position i; if i>=size() p is appended. */
|
||||
void insert_path ( int i, const SrCfgPathBase& p );
|
||||
|
||||
/*! Appends p to the path; p will become an empty path after this call */
|
||||
void append_path ( SrCfgPathBase& p );
|
||||
|
||||
/*! Swap the positions of the two nodes */
|
||||
void swap ( int i, int j );
|
||||
|
||||
/*! Reverts the order of the nodes in the path */
|
||||
void revert ();
|
||||
|
||||
/*! pushes or pops entries until reaching size s */
|
||||
void size ( int s );
|
||||
|
||||
/*! Returns the number of configurations in the path */
|
||||
int size () const { return _size; }
|
||||
|
||||
/*! Returns the length of the path from node i1 to node i2.
|
||||
The length is calculated by adding the distances
|
||||
of adjacent configurations in the path. */
|
||||
float len ( int i1, int i2 ) const;
|
||||
|
||||
/*! Returns the length of the full path. */
|
||||
float len () const { return len(0,_size-1); }
|
||||
|
||||
/*! Returns in c the interpolated configuration in the path according to parameter t,
|
||||
which must be in the closed interval [0,len]. */
|
||||
void interp ( float t, srcfg* c );
|
||||
|
||||
/*! Returns in c the interpolated configuration in the path according to the time
|
||||
parameter t, which must be a valid time between the times associated with the
|
||||
first and last nodes of the path. The configuration manager method time() is
|
||||
used to retrieve the time associated with each configuration. */
|
||||
void temporal_interp ( float t, srcfg* c );
|
||||
|
||||
/*! The smooth routine takes two random configurations interpolated along the path
|
||||
and replaces the portion between them by a direct interpolation if no collisions
|
||||
appear (up to precision prec). Parameter len should contain the current lenght of
|
||||
the path. A <0 value can be given if the length is not known in advance. In any
|
||||
case, after the smooth, len will contain the updated path length. */
|
||||
void smooth_random ( float prec, float& len );
|
||||
|
||||
/*! tries to replace the subpath(t1,t2) by a "straight interpolation".
|
||||
Returns: updated len, 0:same edge, -1:collision, 1:done */
|
||||
int linearize ( float prec, float& len, float t1, float t2 );
|
||||
|
||||
/*! Make one pass in all nodes, trying first to smooth nodes closer to the first and
|
||||
last nodes. Usefull because most often several nodes are created near the root
|
||||
of the two expanding trees. Usually is called once before the random smooths. */
|
||||
void smooth_ends ( float prec );
|
||||
|
||||
void smooth_init ( float prec );
|
||||
bool smooth_step ();
|
||||
|
||||
/*! Copy operator */
|
||||
void operator= ( const SrCfgPathBase& p ) { init(); insert_path(0,p); }
|
||||
|
||||
/*! Returns the configuration of the last node in the path */
|
||||
srcfg* top () { return _buffer[_size-1]; }
|
||||
|
||||
/*! Returns configuration index i */
|
||||
srcfg* get ( int i ) { return _buffer[i]; }
|
||||
|
||||
/*! Const version of get() */
|
||||
const srcfg* const_get ( int i ) const { return _buffer.const_get(i); }
|
||||
|
||||
/*! Outputs the path nodes for inspection */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrCfgPathBase& p );
|
||||
|
||||
private:
|
||||
float _diff ( int i, float prec );
|
||||
};
|
||||
|
||||
/*! This template provides automatic type casts for the user-defined
|
||||
configuration class C. */
|
||||
template <class C>
|
||||
class SrCfgPath : public SrCfgPathBase
|
||||
{ public :
|
||||
SrCfgPath ( SrCfgManagerBase* cman ) : SrCfgPathBase(cman) {}
|
||||
SrCfgPath ( const SrCfgPath& p ) : SrCfgPathBase(p) {}
|
||||
|
||||
C* operator[] ( int i ) { return get(i); }
|
||||
C* top () { return (C*)SrCfgPathBase::top(); }
|
||||
C* get ( int i ) { return (C*)SrCfgPathBase::get(i); }
|
||||
const C* const_get ( int i ) const { return (const C*)SrCfgPathBase::const_get(i); }
|
||||
void operator= ( const SrCfgPath<C>& p ) { SrCfgPathBase::init(); SrCfgPathBase::insert_path(0,p); }
|
||||
};
|
||||
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_PATH_H
|
||||
|
||||
128
source/dcdt/se/sr_cfg_planner.cpp
Normal file
128
source/dcdt/se/sr_cfg_planner.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cfg_planner.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // start
|
||||
//# define SR_USE_TRACE2 // update
|
||||
//# define SR_USE_TRACE3 // bridge
|
||||
# include "sr_trace.h"
|
||||
|
||||
//============================= SrCfgPlannerBase ========================================
|
||||
|
||||
SrCfgPlannerBase::SrCfgPlannerBase ( SrCfgManagerBase* cman )
|
||||
:_tree1 ( cman ), _tree2 ( cman ), _path ( cman )
|
||||
{
|
||||
_cman = cman;
|
||||
|
||||
_tmpc1 = _cman->alloc();
|
||||
_tmpc2 = _cman->alloc();
|
||||
|
||||
_solved = false;
|
||||
_juststarted = false;
|
||||
_curtree = 1;
|
||||
}
|
||||
|
||||
SrCfgPlannerBase::~SrCfgPlannerBase ()
|
||||
{
|
||||
init ();
|
||||
_cman->free ( _tmpc1 );
|
||||
_cman->free ( _tmpc2 );
|
||||
}
|
||||
|
||||
void SrCfgPlannerBase::init ()
|
||||
{
|
||||
_tree1.init ();
|
||||
_tree2.init ();
|
||||
_path.init ();
|
||||
_solved = false;
|
||||
_juststarted = false;
|
||||
_curtree = 1;
|
||||
}
|
||||
|
||||
void SrCfgPlannerBase::start ( const srcfg* c1, const srcfg* c2 )
|
||||
{
|
||||
SR_TRACE1 ( "Start...");
|
||||
init ();
|
||||
|
||||
SR_TRACE1 ( "Tree Init...");
|
||||
_tree1.init ( c1 );
|
||||
_tree2.init ( c2 );
|
||||
_curtree = 1; // could be 1 or 2
|
||||
_juststarted = true;
|
||||
|
||||
SR_TRACE1 ( "Start OK.");
|
||||
}
|
||||
|
||||
bool SrCfgPlannerBase::update_rrt ( float step, int tries, float prec )
|
||||
{
|
||||
SrCfgNode* n; // new node added to the current tree
|
||||
SrCfgNode* nearest1; // nearest node in tree1
|
||||
SrCfgNode* nearest2; // nearest node in tree2
|
||||
float dist1; // distance from crand to nearest1
|
||||
float dist2; // distance from crand to nearest2
|
||||
srcfg* crand = _tmpc1; // the random configuration
|
||||
|
||||
SR_TRACE2 ( "UPDT: expanding tree " << _curtree );
|
||||
|
||||
if ( _juststarted )
|
||||
{ _juststarted = false;
|
||||
if ( _try_to_join(_tree1.root(),_tree2.root(),prec) ) return true; // FOUND
|
||||
}
|
||||
|
||||
_cman->random ( crand );
|
||||
nearest1 = _tree1.search_nearest ( crand, &dist1 );
|
||||
nearest2 = _tree2.search_nearest ( crand, &dist2 );
|
||||
|
||||
float dist = _cman->dist(nearest1->cfg(),nearest2->cfg());
|
||||
if ( dist<=step )
|
||||
{ if ( _try_to_join(nearest1,nearest2,prec) ) return true; // FOUND
|
||||
SR_TRACE2 ( "UPDT: not found." );
|
||||
return false; // not found
|
||||
}
|
||||
|
||||
SR_TRACE2 ( "UPDT: nearest1="<<nearest1->id()<<" nearest2="<<nearest2->id() );
|
||||
SR_TRACE2 ( "UPDT: expanding..." );
|
||||
if ( _curtree==1 )
|
||||
{ if ( _cman->monotone ( nearest1->cfg(), crand ) )
|
||||
{ n = _tree1.expand_node_safe ( nearest1, crand, step, tries, prec, dist1 );
|
||||
if ( n )
|
||||
{ if ( _try_to_join(n,nearest2,prec) ) return true; // FOUND
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{ if ( _cman->monotone ( crand, nearest2->cfg() ) )
|
||||
{ n = _tree2.expand_node_safe ( nearest2, crand, step, tries, prec, dist2 );
|
||||
if ( n )
|
||||
{ if ( _try_to_join(nearest1,n,prec) ) return true; // FOUND
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_curtree = _curtree==1? 2:1;
|
||||
|
||||
SR_TRACE2 ( "UPDT: not found." );
|
||||
|
||||
return false; // not found
|
||||
}
|
||||
|
||||
bool SrCfgPlannerBase::_try_to_join ( SrCfgNode* n1, SrCfgNode* n2, float prec )
|
||||
{
|
||||
bool b;
|
||||
|
||||
b = _cman->monotone ( n1->cfg(), n2->cfg() );
|
||||
if ( !b ) return false;
|
||||
|
||||
b = _cman->visible ( n1->cfg(), n2->cfg(), _tmpc2, prec );
|
||||
if ( !b ) return false;
|
||||
|
||||
_path.init ();
|
||||
_tree1.get_branch ( n1, _path );
|
||||
_path.revert();
|
||||
_tree2.get_branch ( n2, _path );
|
||||
|
||||
_solved = true;
|
||||
return _solved;
|
||||
}
|
||||
|
||||
//============================= End of File ===========================================
|
||||
|
||||
88
source/dcdt/se/sr_cfg_planner.h
Normal file
88
source/dcdt/se/sr_cfg_planner.h
Normal file
@ -0,0 +1,88 @@
|
||||
|
||||
# ifndef SR_CFG_PLANNER_H
|
||||
# define SR_CFG_PLANNER_H
|
||||
|
||||
//# include <SR/sr_cfg_path.h>
|
||||
//# include <SR/sr_cfg_tree.h>
|
||||
# include "sr_heap.h"
|
||||
# include "sr_cfg_path.h"
|
||||
# include "sr_cfg_tree.h"
|
||||
|
||||
/*! A single-query, bidirectional, lazy and sampling-based planner */
|
||||
class SrCfgPlannerBase : public SrSharedClass
|
||||
{ private :
|
||||
SrCfgTreeBase _tree1, _tree2;
|
||||
SrCfgPathBase _path;
|
||||
SrCfgManagerBase* _cman;
|
||||
srcfg* _tmpc1;
|
||||
srcfg* _tmpc2;
|
||||
int _curtree;
|
||||
bool _solved;
|
||||
bool _juststarted;
|
||||
struct HeapEdge { SrCfgTreeBase* tree; SrCfgNode* n1; SrCfgNode* n2; };
|
||||
SrHeap<HeapEdge,int> _heap;
|
||||
|
||||
public :
|
||||
|
||||
/*! The constructor requires a configuration manager. */
|
||||
SrCfgPlannerBase ( SrCfgManagerBase* cman );
|
||||
|
||||
/*! Destructor frees all used internal data, and unref the associated
|
||||
configuration managers */
|
||||
~SrCfgPlannerBase ();
|
||||
|
||||
/*! Returns the roadmap tree rooted at the source configuration */
|
||||
SrCfgTreeBase& tree1 () { return _tree1; }
|
||||
|
||||
/*! Returns the roadmap tree rooted at the destination configuration */
|
||||
SrCfgTreeBase& tree2 () { return _tree2; }
|
||||
|
||||
/*! Returns the number of nodes in both trees */
|
||||
int nodes () const { return _tree1.nodes()+_tree2.nodes(); }
|
||||
|
||||
/*! Clears everything */
|
||||
void init ();
|
||||
|
||||
/*! Clears everything and define the source and goal configurations.
|
||||
Configurations c1 and c2 must be valid.
|
||||
If a time-varying problem will be solved, configuration c1 must correspond
|
||||
to the start and c2 to the goal, ie c1 happens before c2 */
|
||||
void start ( const srcfg* c1, const srcfg* c2 );
|
||||
|
||||
/*! Returns true if a path to the goal was found, and false otherwise. */
|
||||
bool solved () const { return _solved; }
|
||||
|
||||
/*! Returns the last path found by the planner */
|
||||
SrCfgPathBase& path () { return _path; }
|
||||
|
||||
/*! Update one of the trees, returning true if a path was found
|
||||
Parameter step is the incremental step distance, and
|
||||
tries is the number of bisections to try in case of expantion failure */
|
||||
bool update_rrt ( float step, int tries, float prec );
|
||||
|
||||
/*! Lazy version of the update method. */
|
||||
bool update_lazy ( float step, int tries, float prec );
|
||||
|
||||
private :
|
||||
bool _test_bridge ( SrCfgNode* n1, SrCfgNode* n2, float prec );
|
||||
bool _try_to_join ( SrCfgNode* n1, SrCfgNode* n2, float prec );
|
||||
void _heap_add_branch ( SrCfgTreeBase* tree, SrCfgNode* n );
|
||||
};
|
||||
|
||||
/*! Planner template for user-defined configurations */
|
||||
template <class C>
|
||||
class SrCfgPlanner : public SrCfgPlannerBase
|
||||
{ public :
|
||||
SrCfgPlanner ( SrCfgManagerBase* cman )
|
||||
: SrCfgPlannerBase(cman) { }
|
||||
|
||||
SrCfgTree<C>& tree1 () { return (SrCfgTree<C>&) SrCfgPlannerBase::tree1(); }
|
||||
SrCfgTree<C>& tree2 () { return (SrCfgTree<C>&) SrCfgPlannerBase::tree2(); }
|
||||
|
||||
SrCfgPath<C>& path () { return (SrCfgPath<C>&) SrCfgPlannerBase::path(); }
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_CFG_PLANNER_H
|
||||
|
||||
137
source/dcdt/se/sr_cfg_planner_lazy.cpp
Normal file
137
source/dcdt/se/sr_cfg_planner_lazy.cpp
Normal file
@ -0,0 +1,137 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cfg_planner.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // not used
|
||||
//# define SR_USE_TRACE2 // update
|
||||
//# define SR_USE_TRACE3 // bridge
|
||||
# include "sr_trace.h"
|
||||
|
||||
//============================= SrCfgPlannerBase ========================================
|
||||
|
||||
bool SrCfgPlannerBase::update_lazy ( float step, int tries, float prec )
|
||||
{
|
||||
SrCfgNode* n; // new node added to the current tree
|
||||
SrCfgNode* nearest1; // nearest node in tree1
|
||||
SrCfgNode* nearest2; // nearest node in tree2
|
||||
float dist1; // distance from crand to nearest1
|
||||
float dist2; // distance from crand to nearest2
|
||||
srcfg* crand = _tmpc1; // the random configuration
|
||||
|
||||
SR_TRACE2 ( "UPDT: expanding tree " << _curtree );
|
||||
|
||||
if ( _juststarted )
|
||||
{ _juststarted = false;
|
||||
if ( _test_bridge(_tree1.root(),_tree2.root(),prec) ) return true; // FOUND
|
||||
}
|
||||
|
||||
_cman->random ( crand );
|
||||
nearest1 = _tree1.search_nearest ( crand, &dist1 );
|
||||
nearest2 = _tree2.search_nearest ( crand, &dist2 );
|
||||
|
||||
float dist = _cman->dist(nearest1->cfg(),nearest2->cfg());
|
||||
if ( dist<=step )
|
||||
{ if ( _test_bridge(nearest1,nearest2,prec) ) return true; // FOUND
|
||||
}
|
||||
|
||||
SR_TRACE2 ( "UPDT: nearest1="<<nearest1->id()<<" nearest2="<<nearest2->id() );
|
||||
SR_TRACE2 ( "UPDT: expanding..." );
|
||||
if ( _curtree==1 )
|
||||
{ n = _tree1.expand_node ( nearest1, crand, step, tries, dist1 );
|
||||
if ( n )
|
||||
{ if ( _test_bridge(n,nearest2,prec) ) return true; // FOUND
|
||||
}
|
||||
}
|
||||
else
|
||||
{ n = _tree2.expand_node ( nearest2, crand, step, tries, dist2 );
|
||||
if ( n )
|
||||
{ if ( _test_bridge(nearest1,n,prec) ) return true; // FOUND
|
||||
}
|
||||
}
|
||||
|
||||
_curtree = _curtree==1? 2:1;
|
||||
|
||||
SR_TRACE2 ( "UPDT: not found." );
|
||||
|
||||
return false; // not found
|
||||
}
|
||||
|
||||
void SrCfgPlannerBase::_heap_add_branch ( SrCfgTreeBase* tree, SrCfgNode* n )
|
||||
{
|
||||
HeapEdge e;
|
||||
SrCfgNode *parent;
|
||||
while ( n->parent() )
|
||||
{ parent = n->parent();
|
||||
if ( !parent->safe(n->parentlink()) )
|
||||
{ e.tree = tree;
|
||||
e.n1 = parent; // convention: e.n1 is parent of e.n2
|
||||
e.n2 = n;
|
||||
_heap.insert ( e, parent->level(n->parentlink()) );
|
||||
}
|
||||
n = parent; // move to the parent
|
||||
}
|
||||
}
|
||||
|
||||
/*! Test if the path formed by connecting node index n1 of tree 1 with
|
||||
node index n2 of tree2 is a valid path. The test performs collision
|
||||
detection in the edges of the path incrementing their levels. A
|
||||
priority queue is used to first test edges in lower levels.
|
||||
If all edges in the path become safe, a path is formed and true is returned.
|
||||
Otherwise, the edge found to be invalid is deleted, the two trees are
|
||||
updated to keep the remaining edges, and false is returned */
|
||||
bool SrCfgPlannerBase::_test_bridge ( SrCfgNode* n1, SrCfgNode* n2, float prec )
|
||||
{
|
||||
// add the cfg of n2 to tree1:
|
||||
SrCfgNode* n12 = _tree1.add_node ( n1, n2->cfg() );
|
||||
|
||||
// make priority heap where the cost is the edge level
|
||||
// and add all non-safe path edges of tree 1 and tree 2 to the heap
|
||||
SR_TRACE3 ( "BRIDGE: building heap..." );
|
||||
_heap.init();
|
||||
_heap_add_branch ( &_tree1, n12 );
|
||||
_heap_add_branch ( &_tree2, n2 );
|
||||
|
||||
// test and increment the level of the edges in the heap
|
||||
int level;
|
||||
HeapEdge e;
|
||||
while ( _heap.size()>0 )
|
||||
{ level = _heap.lowest_cost();
|
||||
e = _heap.top();
|
||||
|
||||
SR_TRACE3 ( "BRIDGE: heap size="<<_heap.size()<<" level="<<level<<" ..." );
|
||||
|
||||
if ( !e.tree->increment_edge_level ( e.n1, e.n2, prec ) ) break; // collision found
|
||||
|
||||
// remove and reinsert edge if not yet safe:
|
||||
_heap.remove();
|
||||
if ( !e.n1->safe(e.n2->parentlink()) )
|
||||
_heap.insert ( e, level+1 );
|
||||
}
|
||||
|
||||
if ( _heap.size()==0 ) // all edges were safe: path found
|
||||
{ SR_TRACE3 ( "BRIDGE: path found!" );
|
||||
_solved = true;
|
||||
_path.init ();
|
||||
_tree1.get_branch ( n1, _path );
|
||||
_path.revert();
|
||||
_tree2.get_branch ( n2, _path );
|
||||
SR_TRACE3 ( "BRIDGE: path done." );
|
||||
}
|
||||
else // failed, reorganize trees
|
||||
{ SR_TRACE3 ( "BRIDGE: failed, transferring subtrees..." );
|
||||
|
||||
if ( e.tree==&_tree1 )
|
||||
{ //sr_out<<"\nTRANSFER 1: "<<e.n1->id()<<srspc<< e.n2->id()<<srspc<< n12->id()<<srspc<< n2->id()<<srnl;
|
||||
_tree1.transfer_subtree ( e.n1, e.n2, n12/*joint1*/, _tree2, n2/*joint2*/ );
|
||||
}
|
||||
else
|
||||
{ //sr_out<<"\nTRANSFER 2: "<<e.n1->id()<<srspc<< e.n2->id()<<srspc<< n2->id()<<srspc<< n12->id()<<srnl;
|
||||
_tree2.transfer_subtree ( e.n1, e.n2, n2/*joint1*/, _tree1, n12/*joint2*/ );
|
||||
}
|
||||
|
||||
SR_TRACE3 ( "BRIDGE: transfer done." );
|
||||
_solved = false;
|
||||
}
|
||||
|
||||
return _solved;
|
||||
}
|
||||
|
||||
558
source/dcdt/se/sr_cfg_tree.cpp
Normal file
558
source/dcdt/se/sr_cfg_tree.cpp
Normal file
@ -0,0 +1,558 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cfg_tree.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // expand node
|
||||
# include "sr_trace.h"
|
||||
|
||||
//=============================== SrCfgNode ========================================
|
||||
|
||||
float SrCfgNode::prec ( int i ) const
|
||||
{
|
||||
int lev = level ( i );
|
||||
return dist(i)/(float)sr_pow(2,SR_ABS(lev));
|
||||
}
|
||||
|
||||
void SrCfgNode::get_subtree ( SrArray<SrCfgNode*>& nodes )
|
||||
{
|
||||
int i;
|
||||
nodes.push() = this;
|
||||
for ( i=0; i<_children.size(); i++ ) _children[i].node->get_subtree ( nodes );
|
||||
}
|
||||
|
||||
void SrCfgNode::_deledge ( int e )
|
||||
{
|
||||
_children.remove ( e );
|
||||
|
||||
int i, size = _children.size();
|
||||
for ( i=e; i<size; i++ )
|
||||
_children[i].node->_parentlink = i;
|
||||
}
|
||||
|
||||
void SrCfgNode::_fixplinks ()
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<_children.size(); i++ )
|
||||
_children[i].node->_parentlink = i;
|
||||
for ( i=0; i<_children.size(); i++ )
|
||||
_children[i].node->_fixplinks();
|
||||
}
|
||||
|
||||
void SrCfgNode::_reroot ()
|
||||
{
|
||||
if ( !_parent ) return;
|
||||
|
||||
// new parent will have its old parent as new child:
|
||||
SrCfgNode* curnode = this;
|
||||
SrCfgNode* curparent = _parent;
|
||||
int newparentlink = _children.size();
|
||||
int oldparentlink = _parentlink;
|
||||
_children.push() = _parent->_children[oldparentlink];
|
||||
_children.top().node = _parent;
|
||||
_parent = 0;
|
||||
_parentlink = -1;
|
||||
|
||||
// walk towards the old root, swaping old/new parents:
|
||||
int tmp;
|
||||
SrCfgNode* newparent = this;
|
||||
curnode = curparent;
|
||||
curparent = curnode->_parent; // move to parent
|
||||
while ( curparent )
|
||||
{ // modify curnode:
|
||||
curnode->_children[oldparentlink] = curparent->_children[curnode->_parentlink];
|
||||
curnode->_children[oldparentlink].node = curparent; // parent becomes child
|
||||
tmp = oldparentlink;
|
||||
oldparentlink = curnode->_parentlink;
|
||||
curnode->_parentlink = newparentlink;
|
||||
newparentlink = tmp;
|
||||
curnode->_parent = newparent;
|
||||
// move to parent:
|
||||
newparent = curnode;
|
||||
curnode = curparent;
|
||||
curparent = curparent->_parent;
|
||||
}
|
||||
|
||||
// delete extra child of the old root:
|
||||
curnode->_deledge ( oldparentlink );
|
||||
curnode->_parentlink = newparentlink;
|
||||
curnode->_parent = newparent;
|
||||
}
|
||||
|
||||
//============================= SrCfgTreeBase ======================================
|
||||
|
||||
SrCfgTreeBase::SrCfgTreeBase ( SrCfgManagerBase* cman )
|
||||
{
|
||||
_cman = cman;
|
||||
_cman->ref ();
|
||||
_root = 0;
|
||||
}
|
||||
|
||||
SrCfgTreeBase::~SrCfgTreeBase ()
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<_buffer.size(); i++ )
|
||||
{ _cman->free ( _buffer[i]->_cfg );
|
||||
delete _buffer[i];
|
||||
}
|
||||
_cman->unref();
|
||||
}
|
||||
|
||||
bool SrCfgTreeBase::check_all ( SrOutput& o )
|
||||
{
|
||||
int i, j;
|
||||
o << "Starting check:\n";
|
||||
|
||||
if ( !_root ) { o<<"Check ok, but empty.\n"; return true; }
|
||||
|
||||
o << "Buffer size...\n";
|
||||
_nodes.size ( 0 );
|
||||
_root->get_subtree ( _nodes );
|
||||
if ( _nodes.size()!=_buffer.size()-_freepos.size() ) goto error;
|
||||
|
||||
o << "Buffer indices...\n";
|
||||
for ( i=0; i<_buffer.size(); i++ )
|
||||
if ( _buffer[i]->_bufferid!=i ) goto error;
|
||||
|
||||
o << "Parent-child pointers...\n";
|
||||
for ( i=0; i<_nodes.size(); i++ )
|
||||
{ if ( !_nodes[i]->_parent )
|
||||
{ if ( _nodes[i]!=_root ) { o<<"wrong root "; goto error; } }
|
||||
else
|
||||
{ if ( _nodes[i]->_parent->child(_nodes[i]->_parentlink)!=_nodes[i] )
|
||||
{ o<< "wrong parent link n:" <<
|
||||
_nodes[i]->id()<<" p:"<<_nodes[i]->_parent->id()<<srspc; goto error; }
|
||||
}
|
||||
}
|
||||
|
||||
o << "Children pointers...\n";
|
||||
for ( i=0; i<_nodes.size(); i++ )
|
||||
{ for ( j=0; j<_nodes[i]->children(); j++ )
|
||||
{ if ( _nodes[i]->_children[j].node->_parent!=_nodes[i] )
|
||||
{ o<<"wrong child->_parent pointer "; goto error; }
|
||||
if ( _nodes[i]->_children[j].node->_parentlink!=j )
|
||||
{ o<<"wrong child->_parentlink index "<<
|
||||
_nodes[i]->id()<<"/"<<j<<srspc; goto error; }
|
||||
}
|
||||
}
|
||||
|
||||
o << "Check ok.\n";
|
||||
return true;
|
||||
|
||||
error:
|
||||
o << "error!\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::init ()
|
||||
{
|
||||
_root = 0;
|
||||
_freepos.size ( _buffer.size() );
|
||||
int i, max = _buffer.size()-1;
|
||||
for ( i=0; i<=max; i++ ) _freepos[i] = max-i;
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::init ( const srcfg* cfg )
|
||||
{
|
||||
init ();
|
||||
add_node ( 0, cfg, 0 );
|
||||
}
|
||||
|
||||
SrCfgNode* SrCfgTreeBase::_newnode ()
|
||||
{
|
||||
if ( _freepos.size()>0 )
|
||||
{ return _buffer[_freepos.pop()];
|
||||
}
|
||||
else
|
||||
{ int id = _buffer.size();
|
||||
_buffer.push() = new SrCfgNode;
|
||||
_buffer[id]->_cfg = _cman->alloc();
|
||||
_buffer[id]->_bufferid = id;
|
||||
return _buffer[id];
|
||||
}
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::_delnode ( SrCfgNode* n )
|
||||
{
|
||||
_freepos.push() = n->_bufferid;
|
||||
}
|
||||
|
||||
SrCfgNode* SrCfgTreeBase::add_node ( SrCfgNode* parent, const srcfg* cfg, float dist )
|
||||
{
|
||||
SrCfgNode* newn = _newnode ();
|
||||
|
||||
if ( parent ) // set parent data
|
||||
{ newn->_parentlink = parent->children();
|
||||
SrCfgNode::Link& l = parent->_children.push();
|
||||
if ( dist<0 ) dist = _cman->dist ( parent->_cfg, cfg );
|
||||
l.node = newn;
|
||||
l.dist = dist;
|
||||
l.level = 0;
|
||||
}
|
||||
else // this is the root node
|
||||
{ _root = newn;
|
||||
newn->_parentlink = -1;
|
||||
}
|
||||
|
||||
_cman->copy ( newn->_cfg, cfg );
|
||||
newn->_parent = parent;
|
||||
newn->_children.size(0);
|
||||
|
||||
return newn;
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::_nearest ( SrCfgNode* n, const srcfg* c, SrCfgNode*& nearest, float& mindist )
|
||||
{
|
||||
// check distance:
|
||||
float dist = _cman->dist ( n->cfg(), c );
|
||||
if ( dist<mindist )
|
||||
{ nearest = n;
|
||||
mindist = dist;
|
||||
}
|
||||
|
||||
// recurse:
|
||||
int i, chsize=n->children();
|
||||
for ( i=0; i<chsize; i++ )
|
||||
_nearest ( n->child(i), c, nearest, mindist );
|
||||
}
|
||||
|
||||
SrCfgNode* SrCfgTreeBase::search_nearest ( const srcfg* c, float* d )
|
||||
{
|
||||
if ( !_root ) return _root;
|
||||
|
||||
SrCfgNode* nearest;
|
||||
float mindist = 1E+30f; // float range in visualc is: 3.4E +/- 38
|
||||
|
||||
_nearest ( _root, c, nearest, mindist );
|
||||
|
||||
if (d) *d = mindist;
|
||||
return nearest;
|
||||
}
|
||||
|
||||
SrCfgNode* SrCfgTreeBase::expand_node ( SrCfgNode* source, const srcfg* direction,
|
||||
float step, int maxtries, float dist )
|
||||
{
|
||||
srcfg* csource = source->cfg();
|
||||
if ( dist<0 ) dist = _cman->dist ( csource, direction );
|
||||
if ( dist<0.00001f ) return 0; // too close
|
||||
|
||||
SrCfgNode* nnew = _newnode();
|
||||
srcfg* cnew = nnew->cfg();
|
||||
float t = step/dist;
|
||||
if ( t>1.0f ) t = 1.0f;
|
||||
|
||||
while ( maxtries-->0 )
|
||||
{ SR_TRACE1 ( "INS: trying to insert...");
|
||||
|
||||
_cman->interp ( csource, direction, t, cnew );
|
||||
t /= 2;
|
||||
|
||||
if ( _cman->valid(cnew) )
|
||||
{ SR_TRACE1 ( "INS: inserting 1 node.");
|
||||
_delnode ( nnew );
|
||||
return add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
|
||||
}
|
||||
}
|
||||
|
||||
SR_TRACE1 ( "INS: no nodes inserted.");
|
||||
_delnode ( nnew );
|
||||
return 0;
|
||||
}
|
||||
|
||||
SrCfgNode* SrCfgTreeBase::expand_node_safe ( SrCfgNode* source, const srcfg* direction,
|
||||
float step, int maxtries, float prec, float dist )
|
||||
{
|
||||
srcfg* csource = source->cfg();
|
||||
if ( dist<0 ) dist = _cman->dist ( csource, direction );
|
||||
if ( dist<0.00001f ) return 0; // too close
|
||||
|
||||
SrCfgNode* nnew = _newnode();
|
||||
srcfg* cnew = nnew->cfg();
|
||||
SrCfgNode* ntmp = _newnode();
|
||||
srcfg* ctmp = ntmp->cfg();
|
||||
|
||||
float t = step/dist;
|
||||
if ( t>1.0f ) t = 1.0f;
|
||||
|
||||
if ( 0 ) // test "long" expansion
|
||||
{ float dt = t;
|
||||
maxtries=5;
|
||||
while ( t<=1 && maxtries-->0 )
|
||||
{ SR_TRACE1 ( "INS: trying to insert...");
|
||||
_cman->interp ( csource, direction, t, cnew );
|
||||
t += dt;
|
||||
|
||||
if ( _cman->valid(cnew) )
|
||||
if ( _cman->visible(source->cfg(),cnew,ctmp,prec) )
|
||||
{ SR_TRACE1 ( "INS: inserting 1 node.");
|
||||
_delnode ( nnew );
|
||||
_delnode ( ntmp );
|
||||
nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
|
||||
source->_children.top().level = -1; // mark as safe (can be any <0 number)
|
||||
_cman->child_added ( source->cfg(), cnew ); // notify configuration manager
|
||||
SR_TRACE1 ( "INS: Ok.");
|
||||
source = nnew;
|
||||
if ( t>1 ) return nnew; // end
|
||||
|
||||
nnew = _newnode();
|
||||
cnew = nnew->cfg();
|
||||
ntmp = _newnode();
|
||||
ctmp = ntmp->cfg();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
while ( maxtries-->0 )
|
||||
{ SR_TRACE1 ( "INS: trying to insert...");
|
||||
|
||||
_cman->interp ( csource, direction, t, cnew );
|
||||
t /= 2;
|
||||
|
||||
if ( _cman->valid(cnew) )
|
||||
if ( _cman->visible(source->cfg(),cnew,ctmp,prec) )
|
||||
{ SR_TRACE1 ( "INS: inserting 1 node.");
|
||||
_delnode ( nnew );
|
||||
_delnode ( ntmp );
|
||||
nnew = add_node ( source, cnew, -1 ); // instead of -1, could use aprox distance dist*t...
|
||||
source->_children.top().level = -1; // mark as safe (can be any <0 number)
|
||||
_cman->child_added ( source->cfg(), cnew ); // notify configuration manager
|
||||
SR_TRACE1 ( "INS: Ok.");
|
||||
return nnew;
|
||||
//following test not ok:
|
||||
//return expand_node_safe ( nnew, direction, step, maxtries, prec, -1 );
|
||||
}
|
||||
}
|
||||
|
||||
SR_TRACE1 ( "INS: no nodes inserted.");
|
||||
_delnode ( nnew );
|
||||
_delnode ( ntmp );
|
||||
return 0;
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrArray<SrCfgNode*>& nodes )
|
||||
{
|
||||
while ( n )
|
||||
{ nodes.push() = n;
|
||||
n = n->parent();
|
||||
}
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::get_branch ( SrCfgNode* n, SrCfgPathBase& path )
|
||||
{
|
||||
while ( n )
|
||||
{ path.push ( n->cfg() );
|
||||
n = n->parent();
|
||||
}
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::get_nodes ( SrArray<SrCfgNode*>& nodes )
|
||||
{
|
||||
nodes.size(0);
|
||||
if ( !_root ) return;
|
||||
_root->get_subtree ( nodes );
|
||||
}
|
||||
|
||||
// level k : (2^k)+1 tot tests, 2^(k-1) new tests, 2^k segments
|
||||
// safe edge : dist/segments < collision_precision
|
||||
bool SrCfgTreeBase::increment_edge_level ( SrCfgNode* n1, SrCfgNode* n2, float prec )
|
||||
{
|
||||
SrCfgNode::Link& l = n1->_children[n2->parentlink()];
|
||||
SrCfgNode* tmpnode = _newnode();
|
||||
srcfg* ct = tmpnode->cfg();
|
||||
srcfg* ct0 = n1->cfg();
|
||||
srcfg* ct1 = n2->cfg();
|
||||
|
||||
// we will test the next level k:
|
||||
int k = 1 + l.level;
|
||||
//sr_out<<"K: "<<k<<srnl;
|
||||
|
||||
// test the 2^(k-1) new tests of the new level k:
|
||||
float segs = (float) sr_pow ( 2, k );
|
||||
//float dseg = (l.dist/segs<=prec)? prec : 1.0f/segs; // test if last pass
|
||||
float dseg = 1.0f/segs;
|
||||
float dt = dseg*2.0f;
|
||||
float t;
|
||||
for ( t=dseg; t<1.0f; t+=dt )
|
||||
{ //sr_out<<"t: "<<t<<srnl;
|
||||
_cman->interp ( ct0, ct1, t, ct );
|
||||
if ( !_cman->valid(ct) ) { _delnode(tmpnode); return false; }
|
||||
}
|
||||
|
||||
// ok, promote edge to level k and check if it can be marked as safe:
|
||||
l.level = k;
|
||||
//sr_out<<"dist:"<<l.dist<<" segs:"<<segs<<" prec:"<<prec<<srnl;
|
||||
if ( l.dist/segs<=prec ) l.level=-l.level; // mark as safe
|
||||
|
||||
_delnode(tmpnode);
|
||||
return true;
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::transfer_subtree ( SrCfgNode* n1, SrCfgNode* n2, SrCfgNode* joint1,
|
||||
SrCfgTreeBase& tree2, SrCfgNode* joint2 )
|
||||
{
|
||||
// delete edge [n1,n2]:
|
||||
n1->_deledge ( n2->parentlink() );
|
||||
n2->_parent=0; // n2 is now the root of a disconnected subtree, still using _buffer
|
||||
n2->_parentlink=-1;
|
||||
|
||||
// save the nodes of subtree n2:
|
||||
_nodes.size ( 0 );
|
||||
n2->get_subtree ( _nodes );
|
||||
|
||||
// reorder subtree to make joint1 the new root:
|
||||
joint1->_reroot();
|
||||
|
||||
// attach joint1 children as children of joint2:
|
||||
while ( joint1->_children.size() )
|
||||
{ SrCfgNode::Link& l = joint1->_children.pop();
|
||||
l.node->_parent = joint2;
|
||||
l.node->_parentlink = joint2->_children.size();
|
||||
joint2->_children.push() = l;
|
||||
}
|
||||
|
||||
// finally reorganize buffers; all in _nodes change buffer, except joint1 deleted:
|
||||
int tmpi;
|
||||
SrCfgNode *n, *newn, *tmpn;
|
||||
while ( _nodes.size() )
|
||||
{ n = _nodes.pop();
|
||||
_delnode (n);
|
||||
if ( n!=joint1 ) // "swap" with a new entry in tree2.buffer
|
||||
{ newn = tree2._newnode();
|
||||
SR_SWAPT(_buffer[n->_bufferid],tree2._buffer[newn->_bufferid],tmpn);
|
||||
SR_SWAPT(n->_bufferid,newn->_bufferid,tmpi);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrCfgTreeBase::output ( SrOutput& o, bool printcfg, SrCfgNode* n )
|
||||
{
|
||||
int i, j;
|
||||
|
||||
_nodes.size(0);
|
||||
if ( !n ) n = _root;
|
||||
n->get_subtree ( _nodes );
|
||||
|
||||
o << "\nSrCfgTree " << _nodes.size() << srnl;
|
||||
|
||||
for ( i=0; i<_nodes.size(); i++ )
|
||||
{
|
||||
n = _nodes[i];
|
||||
//t._cman->output ( o, t.cfg(i) );
|
||||
o << "Node:" << n->id();
|
||||
if ( n->parent() ) o << " parent:"<<n->parent()->id();
|
||||
else o<<" root ";
|
||||
|
||||
o << " Children:";
|
||||
for ( j=0; j<n->children(); j++ )
|
||||
{ o << srspc << n->child(j)->id()
|
||||
<< ":" << j
|
||||
<< "/" << n->child(j)->parentlink();
|
||||
}
|
||||
|
||||
if ( printcfg )
|
||||
{ o << "\nData: ";
|
||||
_cman->output(o,n->cfg());
|
||||
o <<"\n";
|
||||
}
|
||||
o << srnl;
|
||||
}
|
||||
}
|
||||
|
||||
//================================ friends =================================================
|
||||
|
||||
|
||||
/*
|
||||
void _inpnode ( SrInput& i, SrCfgNode* n );
|
||||
void SrCfgTreeBase::_inpnode ( SrInput& in, SrCfgNode* n )
|
||||
{
|
||||
int i, size;
|
||||
_cman->input ( in, n->_cfg );
|
||||
|
||||
in.get_token(); // "p"
|
||||
in >> n->_parent;
|
||||
in.get_token(); // "c"
|
||||
in >> size;
|
||||
in.get_token(); // ":"
|
||||
|
||||
n->_children.size(size);
|
||||
for ( i=0; i<size; i++ )
|
||||
{ in >> n->_children[i].dist;
|
||||
in >> n->_children[i].node;
|
||||
in >> n->_children[i].level;
|
||||
}
|
||||
}*/
|
||||
|
||||
/*! read the roadmap */
|
||||
/*friend SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t );
|
||||
SrInput& operator>> ( SrInput& in, SrCfgTreeBase& t )
|
||||
{
|
||||
int i, size;
|
||||
|
||||
t.init();
|
||||
|
||||
in.get_token();
|
||||
if ( in.last_token()!="SrCfgTree" ) return in;
|
||||
|
||||
in >> size;
|
||||
|
||||
for ( i=0; i<size; i++ )
|
||||
{
|
||||
// make sure next entry is valid
|
||||
if ( i==t._nodes.size() ) t._pushnode();
|
||||
|
||||
// read node
|
||||
t._cman->input ( in, t.cfg(i) );
|
||||
t._inpnode ( in, t.node(i) );
|
||||
}
|
||||
|
||||
t._nsize = size;
|
||||
|
||||
return in;
|
||||
}
|
||||
*/
|
||||
//================================ End of File =================================================
|
||||
|
||||
|
||||
|
||||
/*
|
||||
This method takes O(k*n) time. It looks into all nodes of the graph,
|
||||
updating the sorted list with the k closest nodes, which is returned.
|
||||
SrArray<SrCfgTreeBase::NodeDist>& SrCfgTreeBase::search_k_nearests ( const srcfg* c, int k )
|
||||
{
|
||||
_snodes.ensure_capacity(k);
|
||||
_snodes.size(0);
|
||||
SrRoadmapNode* n = _graph.first_node();
|
||||
if ( !n ) return _snodes;
|
||||
|
||||
NodeDist nd;
|
||||
int i, worse=-1;
|
||||
|
||||
SrListIterator<SrRoadmapNode> it(n);
|
||||
for ( it.first(); it.inrange(); it.next() )
|
||||
{ if ( it.get()->blocked() ) continue;
|
||||
nd.n = it.get();
|
||||
nd.d = _cman->get_distance ( it.get()->c, c );
|
||||
|
||||
if ( _snodes.size()<k )
|
||||
{ if ( worse<0 )
|
||||
worse=_snodes.size();
|
||||
else
|
||||
if ( nd.d < _snodes[worse].d ) worse=_snodes.size();
|
||||
_snodes.push() = nd;
|
||||
}
|
||||
else if ( nd.d < _snodes[worse].d )
|
||||
{ _snodes[worse] = nd;
|
||||
worse=0;
|
||||
for ( i=1; i<k; i++ )
|
||||
if ( _snodes[i].d > _snodes[worse].d ) worse=i;
|
||||
}
|
||||
}
|
||||
|
||||
if ( _snodes.size()>1 ) _snodes.sort(nd_compare);
|
||||
|
||||
return _snodes;
|
||||
}
|
||||
|
||||
|
||||
|
||||
*/
|
||||
|
||||
169
source/dcdt/se/sr_cfg_tree.h
Normal file
169
source/dcdt/se/sr_cfg_tree.h
Normal file
@ -0,0 +1,169 @@
|
||||
|
||||
# ifndef SR_CFG_TREE_H
|
||||
# define SR_CFG_TREE_H
|
||||
|
||||
# include "sr_set.h"
|
||||
//# include <SR/sr_cfg_manager.h>
|
||||
# include "sr_cfg_manager.h"
|
||||
# include "sr_cfg_path.h"
|
||||
|
||||
class SrCfgTreeBase;
|
||||
|
||||
/*! SrCfgNode is a node of SrCfgTree. Each edge has a
|
||||
level based on the number of collision checks performed:
|
||||
Level TotTests NewTests Segments
|
||||
0 2 0 1
|
||||
1 3 1 2
|
||||
2 5 2 4
|
||||
3 9 4 8
|
||||
k (2^k)+1 2^(k-1) 2^k
|
||||
Safe edge is achieved if dist/(2^k) < collision precision,
|
||||
and level is marked as safe when this occurs */
|
||||
class SrCfgNode
|
||||
{ private :
|
||||
int _bufferid;
|
||||
srcfg* _cfg;
|
||||
SrCfgNode* _parent;
|
||||
int _parentlink;
|
||||
struct Link { SrCfgNode* node; float dist; int level; };
|
||||
SrArray<Link> _children;
|
||||
friend SrCfgTreeBase;
|
||||
public :
|
||||
SrCfgNode* parent () const { return _parent; }
|
||||
int parentlink () const { return _parentlink; } // -1 if root node
|
||||
int children () const { return _children.size(); }
|
||||
srcfg* cfg () const { return _cfg; }
|
||||
SrCfgNode* child ( int i ) const { return _children[i].node; }
|
||||
float prec ( int i ) const; // dist/2^abs(level)
|
||||
float dist ( int i ) const { return _children[i].dist; }
|
||||
int level ( int i ) const { return _children[i].level; } // <0 if safe
|
||||
bool safe ( int i ) const { return level(i)<0?true:false; }
|
||||
void get_subtree ( SrArray<SrCfgNode*>& nodes ); // add the node itself and all subtree
|
||||
int id () const { return _bufferid; }
|
||||
private :
|
||||
void _deledge ( int e );
|
||||
void _fixplinks ();
|
||||
void _reroot ();
|
||||
};
|
||||
|
||||
/*! The tree-based roadmap */
|
||||
class SrCfgTreeBase
|
||||
{ private :
|
||||
SrCfgManagerBase* _cman; // configuration manager
|
||||
SrCfgNode* _root; // the root of the tree
|
||||
SrArray<SrCfgNode*> _buffer; // buffer of nodes
|
||||
SrArray<int> _freepos; // free positions in buffer
|
||||
SrArray<SrCfgNode*> _nodes; // for temporary use
|
||||
public :
|
||||
|
||||
/*! The constructor requires a valid configuration manager for
|
||||
dealing with the user-defined configuration */
|
||||
SrCfgTreeBase ( SrCfgManagerBase* cman );
|
||||
|
||||
/*! Destructor frees all used internal data, and unref the
|
||||
associated configuration manager */
|
||||
~SrCfgTreeBase ();
|
||||
|
||||
/* Returns a pointer to the used configuration manager */
|
||||
SrCfgManagerBase* cman () const { return _cman; }
|
||||
|
||||
/*! Debug tool to test all internal pointers, should always return true */
|
||||
bool check_all ( SrOutput& o );
|
||||
|
||||
/*! Set the tree as an empty tree */
|
||||
void init ();
|
||||
|
||||
/*! Set the current tree to be a tree containing
|
||||
only the given node as the root of the tree. */
|
||||
void init ( const srcfg* cfg );
|
||||
|
||||
/*! Add a node to the tree, as a child to the provided parent.
|
||||
If dist<0 (the default) the parent-cfg distance is retrieved
|
||||
by using the associated configuration manager.
|
||||
The level of the parent-cfg edge is set to 0.
|
||||
Returns the new node created. */
|
||||
SrCfgNode* add_node ( SrCfgNode* parent, const srcfg* cfg, float dist=-1 );
|
||||
|
||||
/*! Returns the number of nodes in the tree */
|
||||
int nodes () const { return _buffer.size()-_freepos.size(); }
|
||||
|
||||
/*! Returns the root node or null if tree empty */
|
||||
SrCfgNode* root () const { return _root; }
|
||||
|
||||
/*! Performs a O(n) search over all nodes and returns the node
|
||||
with the closest configuration to c. Null is returned if the
|
||||
tree is empty. If given pointer d is not null, the nearest
|
||||
distance is returned in d */
|
||||
SrCfgNode* search_nearest ( const srcfg* c, float* d=0 );
|
||||
|
||||
/*! Add a child node from node source, walking a distance of step in direction
|
||||
to configuration direction. If the node is not valid, step is divided
|
||||
by 2 until the node becomes valid, or until max_tries tentatives are performed.
|
||||
Note that here edges are not tested for validity, only nodes (lazy evaluation).
|
||||
The new node is returned, or null in case no node could be added.
|
||||
If parameter dist>0, it will be used as being dist(source->cfg(),direction). */
|
||||
SrCfgNode* expand_node ( SrCfgNode* source, const srcfg* direction, float step, int maxtries, float dist=-1 );
|
||||
|
||||
/*! Same as expand node, but here the expansion is done only if the new edge is
|
||||
valid, therefore the edge visibility tests is called */
|
||||
SrCfgNode* expand_node_safe ( SrCfgNode* source, const srcfg* direction,
|
||||
float step, int maxtries, float prec, float dist=-1 );
|
||||
|
||||
/*! Returns a list of nodes forming the tree branch joining node n to the root.
|
||||
The first element of the array is always n, and the last is always the root node.
|
||||
The array size is not set to zero, ie, the indices are apended to the array. */
|
||||
void get_branch ( SrCfgNode* n, SrArray<SrCfgNode*>& nodes );
|
||||
|
||||
/*! Same as the other get_branch() method, but the result goes to a path object */
|
||||
void get_branch ( SrCfgNode* n, SrCfgPathBase& path );
|
||||
|
||||
/*! Returns an unordered list with all nodes in the tree */
|
||||
void get_nodes ( SrArray<SrCfgNode*>& nodes );
|
||||
|
||||
/*! Performs 2^(k-1) collision tests to check if the edge [n1,n2] can be promoted
|
||||
to level k = n1->level(n2->parentlink())+1.
|
||||
False is returned in case the level could not be promoted due to a collision.
|
||||
True is returned if the level could be promoted, and in this case, the edge
|
||||
will be marked as safe if the new level achieves the required precision
|
||||
with the following test: n1->dist(n2->parentlink())/2^k < prec */
|
||||
bool increment_edge_level ( SrCfgNode* n1, SrCfgNode* n2, float prec );
|
||||
|
||||
/*! Removes the edge [n1,n2] and transfers the disconnected subtree to tree2,
|
||||
by "identifying" joint1 with joint2 of tree2 and reorganizing the subtree
|
||||
so as to make joint2 the subtree root.
|
||||
Important: n1 must be parent of n2, and joint1 must be in n2 subtree */
|
||||
void transfer_subtree ( SrCfgNode* n1, SrCfgNode* n2, SrCfgNode* joint1,
|
||||
SrCfgTreeBase& tree2, SrCfgNode* joint2 );
|
||||
|
||||
/*! Output of the roadmap tree for inspection:
|
||||
- if printcfg is true, node data is also sent to output
|
||||
- n is the root of the subgraph to print (if 0, the real root is taken)*/
|
||||
void output ( SrOutput& o, bool printcfg=true, SrCfgNode* n=0 );
|
||||
|
||||
private :
|
||||
SrCfgNode* _newnode ();
|
||||
void _delnode ( SrCfgNode* n );
|
||||
srcfg* _tmpnode ();
|
||||
void _nearest ( SrCfgNode* n, const srcfg* c, SrCfgNode*& nearest, float& mindist );
|
||||
};
|
||||
|
||||
/*! This template version performs all required type casts to bind the
|
||||
tree to the user-defined configuration class. Class C must be managed
|
||||
by the used configuration manager */
|
||||
template <class C>
|
||||
class SrCfgTree : public SrCfgTreeBase
|
||||
{ public :
|
||||
/*! Constructor receiving a user-defined manager */
|
||||
SrCfgTree ( SrCfgManagerBase* cman ) : SrCfgTreeBase(cman) { }
|
||||
|
||||
/*! Automatically allocates a manager using SrCfgManager<C> template */
|
||||
SrCfgTree () : SrCfgTreeBase ( new SrCfgManager<C> ) { }
|
||||
|
||||
/*! Returns the configuration at node index n */
|
||||
C* cfg ( SrCfgNode* n ) const { return (C*)n->cfg(); }
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_CFG_TREE_H
|
||||
|
||||
59
source/dcdt/se/sr_class_manager.h
Normal file
59
source/dcdt/se/sr_class_manager.h
Normal file
@ -0,0 +1,59 @@
|
||||
|
||||
# ifndef SR_CLASS_MANAGER_H
|
||||
# define SR_CLASS_MANAGER_H
|
||||
|
||||
/** \file sr_class_manager.h
|
||||
* Generic way to allocate, io and compare classes */
|
||||
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
# include "sr_shared_class.h"
|
||||
|
||||
class SrClassManagerBase : public SrSharedClass
|
||||
{ protected :
|
||||
virtual ~SrClassManagerBase() {};
|
||||
|
||||
public : // callbacks
|
||||
virtual void* alloc ()=0;
|
||||
virtual void* alloc ( const void* obj )=0;
|
||||
virtual void free ( void* obj )=0;
|
||||
virtual void output ( SrOutput& o, const void* obj ) { }
|
||||
virtual void input ( SrInput& i, void* obj ) { }
|
||||
virtual int compare ( const void* obj1, const void* obj2 ) { return 0; }
|
||||
};
|
||||
|
||||
/*! Example of an implementation of a class to be automatically managed
|
||||
with SrClassManager<MyData> :
|
||||
class MyData
|
||||
{ public :
|
||||
MyData ();
|
||||
MyData ( const MyData& d );
|
||||
~MyData ();
|
||||
friend SrOutput& operator<< ( SrOutput& out, const MyData& d );
|
||||
friend SrInput& operator>> ( SrInput& inp, MyData& d );
|
||||
friend int sr_compare ( const MyData* d1, const MyData* d2 );
|
||||
};*/
|
||||
|
||||
template <class X>
|
||||
class SrClassManager : public SrClassManagerBase
|
||||
{ protected :
|
||||
virtual ~SrClassManager<X> () {}
|
||||
|
||||
public :
|
||||
virtual void* alloc () { return (void*) new X; }
|
||||
|
||||
virtual void* alloc ( const void* obj ) { return (void*) new X(*((X*)obj)); }
|
||||
|
||||
virtual void free ( void* obj ) { delete (X*) obj; }
|
||||
|
||||
virtual void output ( SrOutput& o, const void* obj ) { o<<*((const X*)obj); }
|
||||
|
||||
virtual void input ( SrInput& i, void* obj ) { i>>*((X*)obj); }
|
||||
|
||||
virtual int compare ( const void* obj1, const void* obj2 )
|
||||
{ return sr_compare ( (const X*)obj1, (const X*)obj2 ); }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_CLASS_MANAGER_H
|
||||
108
source/dcdt/se/sr_color.cpp
Normal file
108
source/dcdt/se/sr_color.cpp
Normal file
@ -0,0 +1,108 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_color.h"
|
||||
|
||||
//========================================= static =======================================
|
||||
|
||||
const SrColor SrColor::black (0,0,0);
|
||||
const SrColor SrColor::red (255,0,0);
|
||||
const SrColor SrColor::green (0,255,0);
|
||||
const SrColor SrColor::yellow (255,255,0);
|
||||
const SrColor SrColor::blue (0,0,255);
|
||||
const SrColor SrColor::magenta(255,0,255);
|
||||
const SrColor SrColor::cyan (0,255,255);
|
||||
const SrColor SrColor::white (255,255,255);
|
||||
const SrColor SrColor::gray (127,127,127);
|
||||
|
||||
//========================================= SrColor =======================================
|
||||
|
||||
void SrColor::set ( srbyte x, srbyte y, srbyte z, srbyte w )
|
||||
{
|
||||
r=x; g=y; b=z; a=w;
|
||||
}
|
||||
|
||||
void SrColor::set ( int x, int y, int z, int w )
|
||||
{
|
||||
r=(srbyte)x;
|
||||
g=(srbyte)y;
|
||||
b=(srbyte)z;
|
||||
a=(srbyte)w;
|
||||
}
|
||||
|
||||
void SrColor::set ( float x, float y, float z, float w )
|
||||
{
|
||||
r = (srbyte) ( x*255.0f );
|
||||
g = (srbyte) ( y*255.0f );
|
||||
b = (srbyte) ( z*255.0f );
|
||||
a = (srbyte) ( w*255.0f );
|
||||
}
|
||||
|
||||
void SrColor::set ( const char* s )
|
||||
{
|
||||
switch ( s[0] )
|
||||
{ case 'b' : *this = s[2]=='a'? black:blue; break;
|
||||
case 'r' : *this = red; break;
|
||||
case 'g' : *this = s[2]=='e'? green:gray; break;
|
||||
case 'y' : *this = yellow; break;
|
||||
case 'm' : *this = magenta; break;
|
||||
case 'c' : *this = cyan; break;
|
||||
case 'w' : *this = white; break;
|
||||
}
|
||||
}
|
||||
|
||||
// we dont do double versions to avoid automatic typecasts complications...
|
||||
void SrColor::get ( float f[4] ) const
|
||||
{
|
||||
f[0] = ((float)r) / 255.0f;
|
||||
f[1] = ((float)g) / 255.0f;
|
||||
f[2] = ((float)b) / 255.0f;
|
||||
f[3] = ((float)a) / 255.0f;
|
||||
}
|
||||
|
||||
void SrColor::get ( int i[4] ) const
|
||||
{
|
||||
i[0] = (int)r;
|
||||
i[1] = (int)g;
|
||||
i[2] = (int)b;
|
||||
i[3] = (int)a;
|
||||
}
|
||||
|
||||
void SrColor::get ( srbyte x[4] ) const
|
||||
{
|
||||
x[0] = r;
|
||||
x[1] = g;
|
||||
x[2] = b;
|
||||
x[3] = a;
|
||||
}
|
||||
|
||||
bool operator == ( const SrColor &c1, const SrColor &c2 )
|
||||
{
|
||||
return c1.r==c2.r && c1.g==c2.g &&c1.b==c2.b && c1.a==c2.a? true:false;
|
||||
}
|
||||
|
||||
bool operator != ( const SrColor &c1, const SrColor &c2 )
|
||||
{
|
||||
return c1.r==c2.r && c1.g==c2.g &&c1.b==c2.b && c1.a==c2.a? false:true;
|
||||
}
|
||||
|
||||
SrColor lerp ( const SrColor &c1, const SrColor &c2, float t )
|
||||
{
|
||||
SrColor c;
|
||||
|
||||
c.r = (srbyte) (SR_LERP ( float(c1.r), float(c2.r), t ) + 0.5f);
|
||||
c.g = (srbyte) (SR_LERP ( float(c1.g), float(c2.g), t ) + 0.5f);
|
||||
c.b = (srbyte) (SR_LERP ( float(c1.b), float(c2.b), t ) + 0.5f);
|
||||
c.a = (srbyte) (SR_LERP ( float(c1.a), float(c2.a), t ) + 0.5f);
|
||||
return c;
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrColor& c )
|
||||
{
|
||||
return o << c.r <<' '<< c.g <<' '<< c.b <<' '<< c.a;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SrColor& c )
|
||||
{
|
||||
return in >> c.r >> c.g >> c.b >> c.a;
|
||||
}
|
||||
|
||||
//=================================== End of File ==========================================
|
||||
99
source/dcdt/se/sr_color.h
Normal file
99
source/dcdt/se/sr_color.h
Normal file
@ -0,0 +1,99 @@
|
||||
|
||||
# ifndef SR_COLOR_H
|
||||
# define SR_COLOR_H
|
||||
|
||||
/** \file sr_color.h
|
||||
* A color definition
|
||||
*/
|
||||
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
|
||||
/*! \class SrColor sr_color.h
|
||||
\brief specifies a color
|
||||
|
||||
SrColor specifies a color using 8 bits for each basic color (red,green,blue)
|
||||
and more 8 bits for the alpha (the opacity). In this way, each component can
|
||||
have a value from 0 to 255 and the total class has a sizeof of 4 bytes.
|
||||
The default constructor initializes with values (r,g,b,a)=(127,127,127,255). */
|
||||
class SrColor
|
||||
{ public :
|
||||
static const SrColor black, //!< black color (0,0,0)
|
||||
red, //!< red color (255,0,0)
|
||||
green, //!< green color (0,255,0)
|
||||
yellow, //!< yellow color (255,255,0)
|
||||
blue, //!< blue color (0,0,255)
|
||||
magenta, //!< magenta color (255,0,255)
|
||||
cyan, //!< cyan color (0,255,255)
|
||||
white, //!< white color (255,255,255)
|
||||
gray; //!< gray color (127,127,127)
|
||||
|
||||
srbyte r; //!< r component, in {0,...,255}, default is 127
|
||||
srbyte g; //!< g component, in {0,...,255}, default is 127
|
||||
srbyte b; //!< b component, in {0,...,255}, default is 127
|
||||
srbyte a; //!< a component, in {0,...,255}, default is 255, that is full opacity
|
||||
|
||||
public :
|
||||
|
||||
/*! Default constructor. Initializes with color gray. */
|
||||
SrColor () { *this=gray; }
|
||||
|
||||
/*! Constructor setting all components. */
|
||||
SrColor ( srbyte x, srbyte y, srbyte z, srbyte w=255 ) { set(x,y,z,w); }
|
||||
|
||||
/*! Constructor setting all components. */
|
||||
SrColor ( int x, int y, int z, int w=255 ) { set(x,y,z,w); }
|
||||
|
||||
/*! Constructor setting all components with float types. */
|
||||
SrColor ( float x, float y, float z, float w=1.0f ) { set(x,y,z,w); }
|
||||
|
||||
/*! Constructor from a 4 dimension float pointer. */
|
||||
SrColor ( const float v[4] ) { set(v); }
|
||||
|
||||
/*! Sets the components of the color, the alpha value has a default parameter of 255. */
|
||||
void set ( srbyte x, srbyte y, srbyte z, srbyte w=255 );
|
||||
|
||||
/*! Sets the components of the color with integer values also betwenn 1 and 255,
|
||||
the alpha value has a default parameter of 255. */
|
||||
void set ( int x, int y, int z, int w=255 );
|
||||
|
||||
/*! Sets the components of the color with float values, each one inside [0.0,1.0],
|
||||
the alpha value has a default parameter of 1.0. */
|
||||
void set ( float x, float y, float z, float w=1.0f );
|
||||
|
||||
/*! Sets the components from and array of four floats. */
|
||||
void set ( const float v[4] ) { set(v[0],v[1],v[2],v[3]); }
|
||||
|
||||
/*! Sets the color with a string containing one of the following:
|
||||
black, red, green, yellow, blue, magenta, cyan, white, gray */
|
||||
void set ( const char* s );
|
||||
|
||||
/*! Put the four components in f[], translating each one to range [0.0,1.0] */
|
||||
void get ( float f[4] ) const;
|
||||
|
||||
/*! Put the four components in i[], each component varying from 0 to 255. */
|
||||
void get ( int i[4] ) const;
|
||||
|
||||
/*! Put the four components in b[], each component varying from 0 to 255. */
|
||||
void get ( srbyte b[4] ) const;
|
||||
|
||||
/*! Comparison equal operator. */
|
||||
friend bool operator == ( const SrColor &c1, const SrColor &c2 );
|
||||
|
||||
/*! Comparison difference operator. */
|
||||
friend bool operator != ( const SrColor &c1, const SrColor &c2 );
|
||||
|
||||
/*! Interpolates two colors. */
|
||||
friend SrColor lerp ( const SrColor &c1, const SrColor &c2, float t );
|
||||
|
||||
/*! Outputs in format 'r g b a'. */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrColor& v );
|
||||
|
||||
/*! Reads from format 'r g b a'. */
|
||||
friend SrInput& operator>> ( SrInput& in, SrColor& v );
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_COLOR_H
|
||||
|
||||
55
source/dcdt/se/sr_cylinder.cpp
Normal file
55
source/dcdt/se/sr_cylinder.cpp
Normal file
@ -0,0 +1,55 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_cylinder.h"
|
||||
# include "sr_box.h"
|
||||
|
||||
//================================== SrCylinder ====================================
|
||||
|
||||
const char* SrCylinder::class_name = "Cylinder";
|
||||
|
||||
SrCylinder::SrCylinder () : a(SrVec::null), b(SrVec::i)
|
||||
{
|
||||
radius = 0.1f;
|
||||
}
|
||||
|
||||
SrCylinder::SrCylinder ( const SrCylinder& c ) : a(c.a), b(c.b)
|
||||
{
|
||||
radius = c.radius;
|
||||
}
|
||||
|
||||
void SrCylinder::get_bounding_box ( SrBox& box ) const
|
||||
{
|
||||
SrVec va = b-a;
|
||||
va.normalize();
|
||||
SrVec vr1;
|
||||
if ( angle(SrVec::i,va)<0.1f )
|
||||
vr1 = cross ( SrVec::j, va );
|
||||
else
|
||||
vr1 = cross ( SrVec::i, va );
|
||||
|
||||
SrVec vr2 = cross ( vr1, va );
|
||||
|
||||
vr1.len ( radius );
|
||||
vr2.len ( radius );
|
||||
|
||||
box.set_empty();
|
||||
box.extend ( a+vr1 );
|
||||
box.extend ( a-vr1 );
|
||||
box.extend ( a+vr2 );
|
||||
box.extend ( a-vr2 );
|
||||
box.extend ( b+vr1 );
|
||||
box.extend ( b-vr1 );
|
||||
box.extend ( b+vr2 );
|
||||
box.extend ( b-vr2 );
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrCylinder& c )
|
||||
{
|
||||
return o << c.a << srspc << c.b << srspc << c.radius;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SrCylinder& c )
|
||||
{
|
||||
return in >> c.a >> c.b >> c.radius;
|
||||
}
|
||||
|
||||
//================================ EOF =================================================
|
||||
46
source/dcdt/se/sr_cylinder.h
Normal file
46
source/dcdt/se/sr_cylinder.h
Normal file
@ -0,0 +1,46 @@
|
||||
|
||||
# ifndef SR_CYLINDER_H
|
||||
# define SR_CYLINDER_H
|
||||
|
||||
/** \file sr_cylinder.h
|
||||
* a cylinder
|
||||
*/
|
||||
|
||||
# include "sr_vec.h"
|
||||
|
||||
class SrBox;
|
||||
|
||||
/*! \class SrCylinder sr_cylinder.h
|
||||
\brief a cylinder
|
||||
|
||||
SrCylinder represents a cylinder based on its endpoints and radius.
|
||||
By default, the cylinder has endpoints (0,0,0) and (1,0,0) and radius 0.1*/
|
||||
class SrCylinder
|
||||
{ public :
|
||||
SrPnt a, b;
|
||||
float radius;
|
||||
static const char* class_name; //!< constain the static string "Cylinder"
|
||||
|
||||
public :
|
||||
|
||||
/*! Constructs a cylinder with endpoints (0,0,0) and (1,0,0) and radius 1 */
|
||||
SrCylinder ();
|
||||
|
||||
/*! Copy constructor */
|
||||
SrCylinder ( const SrCylinder& c );
|
||||
|
||||
/*! Returns the bounding box of all vertices used. The returned box can be empty. */
|
||||
void get_bounding_box ( SrBox &b ) const;
|
||||
|
||||
/*! Outputs in format "p1 p2 radius " */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrCylinder& c );
|
||||
|
||||
/*! Input from format "p1 p2 radius " */
|
||||
friend SrInput& operator>> ( SrInput& in, SrCylinder& c );
|
||||
};
|
||||
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_SCENE_CYLINDER_H
|
||||
|
||||
55
source/dcdt/se/sr_deque.h
Normal file
55
source/dcdt/se/sr_deque.h
Normal file
@ -0,0 +1,55 @@
|
||||
|
||||
# ifndef SR_DEQUE_H
|
||||
# define SR_DEQUE_H
|
||||
|
||||
/** \file sr_deque.h
|
||||
* double-ended queue */
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
/*! \class SrDeque sr_deque.h
|
||||
\brief double-ended queue
|
||||
A double-ended queue templete based on SrArray. */
|
||||
template <class X>
|
||||
class SrDeque // used by the funnel algorithm
|
||||
{ private :
|
||||
SrArray<X> _array;
|
||||
int _base;
|
||||
bool _topmode;
|
||||
|
||||
public :
|
||||
SrDeque () { _base=0; _topmode=true; }
|
||||
|
||||
int size () { return _array.size()-_base; }
|
||||
void init ( int cap ) { _array.ensure_capacity(cap); _array.size(cap/2); _base=_array.size(); }
|
||||
void init () { _array.size(_array.capacity()/2); _base=_array.size(); }
|
||||
void compress () { _array.remove(0,_base); _base=0; _array.compress(); }
|
||||
|
||||
X& top ( int i ) { return _array[_array.size()-i-1]; }
|
||||
X& top () { return _array.top(); }
|
||||
X& popt () { return _array.pop(); }
|
||||
X& pusht () { return _array.push(); }
|
||||
|
||||
X& bottom ( int i ) { return _array[_base+i]; }
|
||||
X& bottom () { return _array[_base]; }
|
||||
X& popb () { return _array[_base++]; }
|
||||
X& pushb ()
|
||||
{ if (_base==0) { _base=_array.size(); _array.insert(0,_base); }
|
||||
return _array[--_base];
|
||||
}
|
||||
|
||||
X& operator[] ( int i ) { return _array[_base+i]; }
|
||||
|
||||
public :
|
||||
bool top_mode () const { return _topmode; }
|
||||
void top_mode ( bool b ) { _topmode=b; }
|
||||
X& get ( int i ) { return _topmode? top(i):bottom(i); }
|
||||
X& get () { return _topmode? top():bottom(); }
|
||||
X& pop () { return _topmode? popt():popb(); }
|
||||
X& push () { return _topmode? pusht():pushb(); }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
#endif // SR_DEQUE_H
|
||||
|
||||
188
source/dcdt/se/sr_euler.cpp
Normal file
188
source/dcdt/se/sr_euler.cpp
Normal file
@ -0,0 +1,188 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_euler.h"
|
||||
# include "sr_mat.h"
|
||||
# include <math.h>
|
||||
|
||||
//# define SR_USE_TRACE1
|
||||
# include "sr_trace.h"
|
||||
|
||||
# define ISZERO(a) ( (a)>-(srtiny) && (a)<(srtiny) )
|
||||
|
||||
# define EQUAL(a,b) ( ( (a)>(b)? ((a)-(b)):((b)-(a)) )<=(srtiny) )
|
||||
|
||||
# define GETSINCOS double cx=cos(rx); double cy=cos(ry); double cz=cos(rz); \
|
||||
double sx=sin(rx); double sy=sin(ry); double sz=sin(rz)
|
||||
|
||||
# define ATAN2(x,y) (float) atan2 ( (double)x, (double)y )
|
||||
# define NORM(x,y) sqrt(double(x)*double(x)+double(y)*double(y))
|
||||
|
||||
//============================ Get Angles ================================
|
||||
|
||||
void sr_euler_angles ( int order, const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
switch ( order )
|
||||
{ case 123: sr_euler_angles_xyz(m,rx,ry,rz); break;
|
||||
case 132: sr_euler_angles_xzy(m,rx,ry,rz); break;
|
||||
case 213: sr_euler_angles_yxz(m,rx,ry,rz); break;
|
||||
case 231: sr_euler_angles_yzx(m,rx,ry,rz); break;
|
||||
case 312: sr_euler_angles_zxy(m,rx,ry,rz); break;
|
||||
case 321: sr_euler_angles_zyx(m,rx,ry,rz); break;
|
||||
default: rx=ry=rz=0;
|
||||
}
|
||||
}
|
||||
|
||||
void sr_euler_angles_xyz ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
ry = ATAN2 ( -m[2], NORM(m[0],m[1]) );
|
||||
rx = ATAN2 ( m[6], m[10] );
|
||||
rz = ATAN2 ( m[1], m[0] );
|
||||
}
|
||||
|
||||
void sr_euler_angles_xzy ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
rx = ATAN2 ( -m[9], m[5] );
|
||||
ry = ATAN2 ( -m[2], m[0] );
|
||||
rz = ATAN2 ( m[1], NORM(m[5],m[9]) );
|
||||
}
|
||||
|
||||
void sr_euler_angles_yxz ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
rx = ATAN2 ( m[6], NORM(m[2],m[10]) );
|
||||
ry = ATAN2 ( -m[2], m[10] );
|
||||
rz = ATAN2 ( -m[4], m[5] );
|
||||
}
|
||||
|
||||
void sr_euler_angles_yzx ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
rz = ATAN2 ( -m[4], NORM(m[0],m[8]) );
|
||||
rx = ATAN2 ( m[6], m[5] );
|
||||
ry = ATAN2 ( m[8], m[0] );
|
||||
}
|
||||
|
||||
void sr_euler_angles_zxy ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
rx = ATAN2 ( -m[9], NORM(m[1],m[5]) );
|
||||
rz = ATAN2 ( m[1], m[5] );
|
||||
ry = ATAN2 ( m[8], m[10] );
|
||||
}
|
||||
|
||||
void sr_euler_angles_zyx ( const SrMat& m, float& rx, float& ry, float& rz )
|
||||
{
|
||||
ry = ATAN2 ( m[8], NORM(m[0],m[4]) );
|
||||
rx = ATAN2 ( -m[9], m[10] );
|
||||
rz = ATAN2 ( -m[4], m[0] );
|
||||
}
|
||||
|
||||
//============================ Get Mat ================================
|
||||
|
||||
void sr_euler_mat ( int order, SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
switch ( order )
|
||||
{ case 123: sr_euler_mat_xyz(m,rx,ry,rz); break;
|
||||
case 132: sr_euler_mat_xzy(m,rx,ry,rz); break;
|
||||
case 213: sr_euler_mat_yxz(m,rx,ry,rz); break;
|
||||
case 231: sr_euler_mat_yzx(m,rx,ry,rz); break;
|
||||
case 312: sr_euler_mat_zxy(m,rx,ry,rz); break;
|
||||
case 321: sr_euler_mat_zyx(m,rx,ry,rz); break;
|
||||
default: m.identity();
|
||||
}
|
||||
}
|
||||
|
||||
void sr_euler_mat_xyz ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Rx*Ry*Rz (in column-major format)
|
||||
m[0]=float(cy*cz); m[1]=float(cy*sz); m[2]=float(-sy);
|
||||
m[4]=float(-cx*sz+sx*sy*cz); m[5]=float(cx*cz+sx*sy*sz); m[6]=float(sx*cy);
|
||||
m[8]=float(sx*sz+cx*sy*cz); m[9]=float(-sx*cz+cx*sy*sz); m[10]=float(cx*cy);
|
||||
}
|
||||
|
||||
void sr_euler_mat_xzy ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Rx*Rz*Ry (in column-major format)
|
||||
m[0]=float(cy*cz); m[1]=float(sz); m[2]=float(-cz*sy);
|
||||
m[4]=float(-cx*sz*cy+sx*sy); m[5]=float(cx*cz); m[6]=float(cx*sy*sz+sx*cy);
|
||||
m[8]=float(sx*sz*cy+cx*sy); m[9]=float(-sx*cz); m[10]=float(-sx*sy*sz+cx*cy);
|
||||
}
|
||||
|
||||
void sr_euler_mat_yxz ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Ry*Rx*Rz (in column-major format)
|
||||
m[0]=float(cy*cz-sx*sy*sz); m[1]=float(cy*sz+sx*sy*cz); m[2]=float(-cx*sy);
|
||||
m[4]=float(-cx*sz); m[5]=float(cx*cz); m[6]=float(sx);
|
||||
m[8]=float(cz*sy+sx*cy*sz); m[9]=float(sy*sz-sx*cy*cz); m[10]=float(cx*cy);
|
||||
}
|
||||
|
||||
void sr_euler_mat_yzx ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Ry*Rz*Rx (in column-major format)
|
||||
m[0]=float(cy*cz); m[1]=float(cx*cy*sz+sx*sy); m[2]=float(cy*sz*sx-cx*sy);
|
||||
m[4]=float(-sz); m[5]=float(cx*cz); m[6]=float(cz*sx);
|
||||
m[8]=float(sy*cz); m[9]=float(cx*sy*sz-cy*sx); m[10]=float(sx*sy*sz+cx*cy);
|
||||
}
|
||||
|
||||
void sr_euler_mat_zxy ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Rz*Rx*Ry (in column-major format)
|
||||
m[0]=float(cy*cz+sx*sy*sz); m[1]=float(cx*sz); m[2]=float(-sy*cz+sx*cy*sz);
|
||||
m[4]=float(-sz*cy+sx*sy*cz); m[5]=float(cx*cz); m[6]=float(sy*sz+sx*cy*cz);
|
||||
m[8]=float(cx*sy); m[9]=float(-sx); m[10]=float(cx*cy);
|
||||
}
|
||||
|
||||
void sr_euler_mat_zyx ( SrMat& m, float rx, float ry, float rz )
|
||||
{
|
||||
GETSINCOS;
|
||||
// the following is the same as: R=Rz*Ry*Rx (in column-major format)
|
||||
m[0]=float(cy*cz); m[1]=float(cx*sz+cz*sy*sx); m[2]=float(sx*sz-cx*cz*sy);
|
||||
m[4]=float(-sz*cy); m[5]=float(cx*cz-sx*sy*sz); m[6]=float(sx*cz+cx*sy*sz);
|
||||
m[8]=float(sy); m[9]=float(-sx*cy); m[10]=float(cx*cy);
|
||||
}
|
||||
|
||||
//================================ EOF ===================================
|
||||
|
||||
/* Math:
|
||||
|1 0 0| |cy 0 -sy| | cz sz 0|
|
||||
Rx=|0 cx sx| Ry=| 0 1 0| Rz=|-sz cz 0|
|
||||
|0 -sx cx| |sy 0 cy| | 0 0 1|
|
||||
|
||||
| cy 0 -sy| |cycz cysz -sy| | cycz sz -czsy|
|
||||
RxRy = |sxsy cx sxcy| RyRz = | -sz cz 0| RzRy = |-szcy cz sysz|
|
||||
|cxsy -sx cxcy| |sycz sysz cy| | sy 0 cy|
|
||||
|
||||
| cycz cysz -sy | | m0 m1 m2 |
|
||||
RxRyRz = |-cxsz+sxsycz cxcz+sxsysz sx*cy | = | m4 m5 m6 |
|
||||
| sxsz+cxsycz -sxcz+cxsysz cx*cy | | m8 m9 m10|
|
||||
X: sx/cx = m6/m10
|
||||
Z: sz/cz = m1/m0
|
||||
Y: cy^2*cz^2 + cy^2*sz^2 = m0^2+m1^2 => cy = norm(m0,m1) => sy/cy = -m2/norm(m0,m1)
|
||||
|
||||
| cycz sz -czsy | | m0 m1 m2 |
|
||||
RxRzRy = | -cxszcy+sxsy cxcz cxsysz+sxcy | = | m4 m5 m6 |
|
||||
| sxszcy+cxsy -sxcz -sxsysz+cxcy | | m8 m9 m10|
|
||||
X: -sx/cx = m9/m5
|
||||
Y: -sy/cy = m2/m0
|
||||
Z: cx^2*cz^2 + cz^2*sx^2 = m5^2+m9^2 => cz = norm(m5,m9) => sz/cz = m1/norm(m5,m9)
|
||||
|
||||
| cycz+sxsysz cxsz -sycz+sxcysz | | m0 m1 m2 |
|
||||
RzRxRy = | -szcy+sxsycz cxcz sysz+sxcycz | = | m4 m5 m6 |
|
||||
| cxsy -sx cxcy | | m8 m9 m10|
|
||||
Z: sz/cz = m1/m5
|
||||
Y: sy/cy = m8/m10
|
||||
X: cx^2*sz^2 + cx^2*cz^2 = m1^2+m5^2 => cx = norm(m1,m5) => sx/cx = -m9/norm(m1,m5)
|
||||
|
||||
| cycz cxsz+czsysx sxsz-cxczsy | | m0 m1 m2 |
|
||||
RzRyRx = | -szcy cxcz-sxsysz sxcz+cxsysz | = | m4 m5 m6 |
|
||||
| sy -sxcy cxcy | | m8 m9 m10|
|
||||
X: -sx/cx = m9/m10
|
||||
Z: -sz/cz = m4/m0
|
||||
Y: cy^2*cz^2 + sz^2*cy^2 = m0^2+m4^2 => cy = norm(m0,m4) => sy/cy = m8/norm(m0,m4)
|
||||
|
||||
(Euler is fixed axis, the "inver order" becomes moving axis)
|
||||
*/
|
||||
|
||||
//================================ EOF ===================================
|
||||
|
||||
41
source/dcdt/se/sr_euler.h
Normal file
41
source/dcdt/se/sr_euler.h
Normal file
@ -0,0 +1,41 @@
|
||||
/** \file sr_euler.h
|
||||
* euler angles tools */
|
||||
|
||||
# ifndef SR_EULER_H
|
||||
# define SR_EULER_H
|
||||
|
||||
//================================ Angles from Mat ================================
|
||||
|
||||
class SrMat;
|
||||
|
||||
/*! Extract from the rotation matrix m the x,y,z Euler angles,
|
||||
assuming they were encoded in m in the given order.
|
||||
Parameter order should be: 123 for XYZ, 132 for XZY, etc.
|
||||
The other sr_euler_angles* functions are similar, but specific to the desired order. */
|
||||
void sr_euler_angles ( int order, const SrMat& m, float& rx, float& ry, float& rz );
|
||||
|
||||
void sr_euler_angles_xyz ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
void sr_euler_angles_xzy ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
void sr_euler_angles_yxz ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
void sr_euler_angles_yzx ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
void sr_euler_angles_zxy ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
void sr_euler_angles_zyx ( const SrMat& m, float& rx, float& ry, float& rz );
|
||||
|
||||
//================================ Mat from Angles ================================
|
||||
|
||||
/*! Set the rotation matrix m according to the given x,y,z Euler angles.
|
||||
Parameter order should be: 123 for XYZ, 132 for XZY, etc.
|
||||
Note: the 4th line and column of m are not changed.
|
||||
The other sr_euler_mat* functions are similar, but specific to the desired order. */
|
||||
void sr_euler_mat ( int order, SrMat& m, float rx, float ry, float rz );
|
||||
|
||||
void sr_euler_mat_xyz ( SrMat& m, float rx, float ry, float rz );
|
||||
void sr_euler_mat_xzy ( SrMat& m, float rx, float ry, float rz );
|
||||
void sr_euler_mat_yxz ( SrMat& m, float rx, float ry, float rz );
|
||||
void sr_euler_mat_yzx ( SrMat& m, float rx, float ry, float rz );
|
||||
void sr_euler_mat_zxy ( SrMat& m, float rx, float ry, float rz );
|
||||
void sr_euler_mat_zyx ( SrMat& m, float rx, float ry, float rz );
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_EULER_H
|
||||
62
source/dcdt/se/sr_event.cpp
Normal file
62
source/dcdt/se/sr_event.cpp
Normal file
@ -0,0 +1,62 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_event.h"
|
||||
|
||||
//# define SR_USE_TRACE1
|
||||
# include "sr_trace.h"
|
||||
|
||||
//===================================== SrEvent =================================
|
||||
|
||||
SrEvent::SrEvent ()
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
void SrEvent::init ()
|
||||
{
|
||||
type = None;
|
||||
key = 0;
|
||||
button = 0;
|
||||
button1 = button2 = button3 = 0;
|
||||
ctrl = shift = alt = key = 0;
|
||||
width = heigth = 0;
|
||||
pixel_size = 0.05f;
|
||||
}
|
||||
|
||||
void SrEvent::init_lmouse ()
|
||||
{
|
||||
type = None;
|
||||
key = 0;
|
||||
button = 0;
|
||||
button1 = button2 = button3 = 0;
|
||||
ctrl = shift = alt = key = 0;
|
||||
lmouse = mouse;
|
||||
lmouse = mouse;
|
||||
mouse.x = mouse.y = 0;
|
||||
}
|
||||
|
||||
const char *SrEvent::type_name () const
|
||||
{
|
||||
switch ( type )
|
||||
{ case None : return "none";
|
||||
case Push : return "push";
|
||||
case Drag : return "drag";
|
||||
case Release : return "release";
|
||||
case Keyboard : return "keyboard";
|
||||
}
|
||||
return "undefined?!";
|
||||
}
|
||||
|
||||
SrOutput& operator<< ( SrOutput& out, const SrEvent& e )
|
||||
{
|
||||
out << e.type_name();
|
||||
|
||||
if ( e.type==SrEvent::Keyboard )
|
||||
out << " [" << (e.key? e.key:' ') << ':' << (int)e.key << ']';
|
||||
|
||||
out << " POS:" << e.mouse <<
|
||||
" BUTS:" << (int)e.button1<<(int)e.button2<<(int)e.button3<<
|
||||
" ACS:" << (int)e.alt<<(int)e.ctrl<<(int)e.shift;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
95
source/dcdt/se/sr_event.h
Normal file
95
source/dcdt/se/sr_event.h
Normal file
@ -0,0 +1,95 @@
|
||||
|
||||
# ifndef SR_EVENT_H
|
||||
# define SR_EVENT_H
|
||||
|
||||
/** \file sr_event.h
|
||||
* A user-generated window-event
|
||||
*/
|
||||
|
||||
# include "sr_vec2.h"
|
||||
# include "sr_line.h"
|
||||
# include "sr_output.h"
|
||||
|
||||
/*! \class SrEvent sr_event.h
|
||||
\brief Keeps a window event
|
||||
|
||||
SrEvent is used to describe a mouse or keyboard event, in a
|
||||
system-independent way. */
|
||||
class SrEvent
|
||||
{ public :
|
||||
/*! Enumerators for the type of event. */
|
||||
enum Type { None, //!< No event occured.
|
||||
Push, //!< A mouse button was pushed.
|
||||
Drag, //!< The mouse moved with a button down.
|
||||
Release, //!< A mouse button was released.
|
||||
Keyboard //!< A key was pressed.
|
||||
};
|
||||
|
||||
/*! Enumerators with codes for special keys. */
|
||||
enum KeyCodes { KeyEsc=65307, KeyDel=65535, KeyIns=65379, KeyBack=65288,
|
||||
KeyUp=65362, KeyDown=65364, KeyLeft=65361, KeyRigth=65363,
|
||||
KeyEnd=65367, KeyPgDn=65366, KeyPgUp=65365, KeyHome=65360,
|
||||
KeyShift=653505, KeyCtrl=65307, KeyAlt=65313, KeySpace=32,
|
||||
KeyEnter=65293 };
|
||||
|
||||
public : //--- event occured :
|
||||
Type type; //!< The type of the occured event
|
||||
char button; //!< The button number 1, 2 or 3 if event type was Push or Release, 0 otherwise
|
||||
int key; //!< The ascii code / code of the key pressed (uppercase) if it was a keyboard event, 0 otherwise
|
||||
int width; //!< The width of the screen when the event occured
|
||||
int heigth; //!< The heigth of the screen when the event occured
|
||||
// float scenew; //!< Width in the units of the scene
|
||||
// float sceneh; //!< Heigth in the units of the scene
|
||||
|
||||
public : //--- states at event time :
|
||||
char button1; //!< Contains 1 if the left mouse button state is pressed, 0 otherwise
|
||||
char button2; //!< Contains 1 if the middle mouse button state is pressed, 0 otherwise
|
||||
char button3; //!< Contains 1 if the right mouse button state is pressed, 0 otherwise
|
||||
char alt; //!< Contains 1 if the Alt modifier key state is pressed, 0 otherwise
|
||||
char ctrl; //!< Contains 1 if the Ctrl modifier key state is pressed, 0 otherwise
|
||||
char shift; //!< Contains 1 if the Shift modifier key state is pressed, 0 otherwise
|
||||
|
||||
public : //--- mouse coordinates :
|
||||
SrVec2 mouse; //!< Current mouse push position in normalized coords [-1,1]
|
||||
SrVec2 lmouse; //!< Last mouse x push position in normalized coords [-1,1]
|
||||
|
||||
public : //--- scene information :
|
||||
SrLine ray;
|
||||
SrLine lray;
|
||||
SrPnt mousep; //!< mouse point in scene coordinates at plane z=0
|
||||
SrPnt lmousep; //!< last mouse point in scene coordinates at plane z=0
|
||||
float pixel_size; //!< 0.05 by default but should be updated according to the camera
|
||||
|
||||
public : //--- methods :
|
||||
|
||||
/*! Initialize as a None event type, by calling init(). */
|
||||
SrEvent ();
|
||||
|
||||
/*! Makes the event as the None type, and puts all data with their default values of zero. */
|
||||
void init ();
|
||||
|
||||
/*! Puts mouse keyboard information to their default value, but saves the mouse
|
||||
values in the lmouse variables. */
|
||||
void init_lmouse ();
|
||||
|
||||
/*! Returns a string with the name of the event type. */
|
||||
const char *type_name () const;
|
||||
|
||||
/*! Returns the difference: mousex-lmousex. */
|
||||
float mousedx () const { return mouse.x-lmouse.x; }
|
||||
|
||||
/*! Returns the difference: mousey-lmousey. */
|
||||
float mousedy () const { return mouse.y-lmouse.y; }
|
||||
|
||||
/*! Returns true if the event type is push, drag, or release; and false otherwise. */
|
||||
bool mouse_event () const { return type==Push||type==Drag||type==Release? true:false; }
|
||||
|
||||
/*! Outputs data of this event for data inspection. */
|
||||
friend SrOutput& operator<< ( SrOutput& out, const SrEvent& e );
|
||||
};
|
||||
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_EVENT_H
|
||||
|
||||
511
source/dcdt/se/sr_exp_table.cpp
Normal file
511
source/dcdt/se/sr_exp_table.cpp
Normal file
@ -0,0 +1,511 @@
|
||||
#include "precompiled.h"//***************************************************************************
|
||||
//
|
||||
// SrExpTABLE.H
|
||||
// By Marcelo Kallmann 08/98 - Brazil
|
||||
//
|
||||
//***************************************************************************
|
||||
|
||||
# include <math.h>
|
||||
# include <string.h>
|
||||
# include <stdlib.h>
|
||||
|
||||
# include "sr_array.h"
|
||||
# include "sr_string.h"
|
||||
# include "sr_exp_table.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // Parse
|
||||
//# define SR_USE_TRACE2 // Eval
|
||||
//# define SR_USE_TRACE3 // Table
|
||||
# include "sr_trace.h"
|
||||
|
||||
// ============================= EToken ========================================
|
||||
|
||||
typedef SrExpTable::Token EToken;
|
||||
|
||||
inline void setNumb ( EToken& t, double d ) { t.type=(char)EToken::Numb; t.data.realval=d; }
|
||||
inline void setVar ( EToken& t, int id ) { t.type=(char)EToken::Var; t.data.index=id; }
|
||||
inline void setFunc ( EToken& t, int id ) { t.type=(char)EToken::Func; t.data.index=id; }
|
||||
inline void setBinOp ( EToken& t, char op ) { t.type=(char)EToken::BinOp; t.data.code=op; }
|
||||
inline void setUnaOp ( EToken& t, char op ) { t.type=(char)EToken::UnaOp; t.data.code=op; }
|
||||
inline void setLeftPar ( EToken& t ) { t.type=(char)EToken::LeftPar; }
|
||||
inline void setRightPar ( EToken& t ) { t.type=(char)EToken::RightPar; }
|
||||
|
||||
//======================= Modes while parsing =================================
|
||||
|
||||
enum srParMode { srParModeUnary, srParModeBinary, srParModeFunction };
|
||||
|
||||
//======================== functions methods ===================================
|
||||
|
||||
enum srFunc { srFuncAbs, srFuncAcos, srFuncAsin, srFuncAtan, srFuncCeil,
|
||||
srFuncCos, srFuncExp, srFuncFact, srFuncFloor, srFuncLn,
|
||||
srFuncLog, srFuncRound, srFuncSign, srFuncSin, srFuncSqrt,
|
||||
srFuncTan, srFuncUndefined }; // Alphabetical order !
|
||||
|
||||
static char *Functions[] = { "abs", "acos", "asin", "atan", "ceil",
|
||||
"cos", "exp", "fact", "floor", "ln",
|
||||
"log", "round", "sign", "sin", "sqrt",
|
||||
"tan" }; // Alphabetical order !
|
||||
|
||||
static int compfunc ( const void *a, const void *b )
|
||||
{
|
||||
return strcmp ( ((char*)a), *((char**)b) );
|
||||
}
|
||||
|
||||
static srFunc func_code ( const char *st )
|
||||
{
|
||||
void *result = bsearch ( (const void*)st, // to search
|
||||
(const void*)Functions, // table
|
||||
(size_t)srFuncUndefined, // num elems in table
|
||||
(size_t)sizeof(char*), // size of each elem in table
|
||||
compfunc // compare function
|
||||
);
|
||||
if (!result) return srFuncUndefined;
|
||||
return (srFunc) ( ((int)result-(int)Functions)/sizeof(int) );
|
||||
}
|
||||
|
||||
const char *SrExpTable::function_name ( int index )
|
||||
{
|
||||
if ( index<0 || index>=(int)srFuncUndefined ) return 0;
|
||||
return Functions[index];
|
||||
}
|
||||
|
||||
static int oprank ( char op )
|
||||
{
|
||||
switch ( op )
|
||||
{ case '[' : return 8;
|
||||
case '^' : case '%' : return 7;
|
||||
case '/' : case '*' : case '|' : return 6;
|
||||
case '+' : case '-' : return 5;
|
||||
case '>' : case '<' :
|
||||
case ')' : case '(' : // >= <=
|
||||
case '~' : case '=' : return 4;
|
||||
case 'a' : case 'o' : return 3; // and or
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//===================================== messages ==============================
|
||||
// Global functions :
|
||||
|
||||
SrExpTable::Msg SrExpTable::translate_error ( SrInput::ErrorType e )
|
||||
{
|
||||
switch ( e )
|
||||
{ case SrInput::UndefChar : return MsgUndefChar;
|
||||
case SrInput::TooBig : return MsgNameTooBig;
|
||||
case SrInput::OpenString : return MsgStringNotClosed;
|
||||
case SrInput::InvalidPoint : return MsgMisplacedDecPoint;
|
||||
default : return MsgOk;
|
||||
}
|
||||
}
|
||||
|
||||
const char* SrExpTable::msg_desc ( SrExpTable::Msg m )
|
||||
{
|
||||
switch ( m )
|
||||
{ // messages translated from sr_input.h :
|
||||
case MsgUndefChar : return SrInput::error_desc ( SrInput::UndefChar );
|
||||
case MsgNameTooBig : return SrInput::error_desc ( SrInput::TooBig );
|
||||
case MsgStringNotClosed : return SrInput::error_desc ( SrInput::OpenString );
|
||||
case MsgMisplacedDecPoint : return SrInput::error_desc ( SrInput::InvalidPoint );
|
||||
// new messages :
|
||||
case MsgOk : return "Ok";
|
||||
case MsgUnexpectedEndOfFile : return "Unexpected end of file encountered";
|
||||
case MsgUndefinedName : return "Undefined variable";
|
||||
case MsgUndefBinOp : return "Undefined binary operator: internal error";
|
||||
case MsgUndefUnaOp : return "Undefined unary operator: internal error";
|
||||
case MsgUndefPreDefFunc : return "Undefined pre-defined function: internal error";
|
||||
case MsgOperandExpected : return "Operand expected in expression";
|
||||
case MsgOperatorExpected : return "Operator expected in expression";
|
||||
case MsgFuncWithoutPar : return "Function name must be followed by a left parenthesis";
|
||||
case MsgDivideByZero : return "Divide by zero";
|
||||
case MsgRaisedZeroToNeg : return "Cannot raise zero to a negative power";
|
||||
case MsgRaisedNegToReal : return "Cannot raise a negative number to a non-integer power";
|
||||
case MsgUnmatchedParentesis : return "Unmatched parentesis";
|
||||
case MsgAcosArgsOutOfRange : return "Arc cos arguments out of range";
|
||||
case MsgAsinArgsOutOfRange : return "Arc sin arguments out of range";
|
||||
case MsgStackEmpty : return "Stack is empty";
|
||||
case MsgSqrtArgIsNeg : return "Sqrt argument is negative";
|
||||
default : return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
//============================ internal ======================================
|
||||
|
||||
# define ISBINARY(c) strchr("+-*/^%|><=[",c)
|
||||
# define ISUNARY(c) strchr("+-!",c)
|
||||
|
||||
static bool is_binary ( const SrString& token )
|
||||
{
|
||||
if ( strchr("+-*/^%|><~=[",token[0]) ) return true;
|
||||
if ( token=="and" || token=="or" ) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
static SrExpTable::Msg get_translated_token ( SrInput& in,
|
||||
int& curline,
|
||||
EToken& etok,
|
||||
srParMode mode,
|
||||
const char* delimeters,
|
||||
SrExpTableVarManager* varman )
|
||||
{
|
||||
SrInput::TokenType toktype;
|
||||
SrString token;
|
||||
|
||||
toktype = in.get_token(token);
|
||||
curline = in.curline();
|
||||
if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() );
|
||||
if ( toktype==SrInput::EndOfFile ) return SrExpTable::MsgUnexpectedEndOfFile;
|
||||
|
||||
if ( delimeters && toktype==SrInput::Delimiter && strchr(delimeters,token[0]) )
|
||||
{ in.unget_token ( token, toktype );
|
||||
return SrExpTable::MsgUnexpectedEndOfFile; // permits to stop the parser before end of file
|
||||
}
|
||||
|
||||
if ( mode==srParModeUnary && ISUNARY(token[0]) )
|
||||
{ setUnaOp ( etok, token[0] );
|
||||
SR_TRACE1 ( "Got a unary op: ["<<token<<"]" );
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
if ( (mode==srParModeBinary || mode==srParModeFunction) && is_binary(token) )
|
||||
{ char op = token[0];
|
||||
if ( op=='>' || op=='<' )
|
||||
{ toktype = in.get_token(token);
|
||||
if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() );
|
||||
curline = in.curline();
|
||||
if ( op=='>' && token[0]=='=' ) op = ')'; // that will mean >=
|
||||
else
|
||||
if ( op=='<' && token[0]=='=' ) op = '('; // that will mean <=
|
||||
else
|
||||
if ( op=='<' && token[0]=='>' ) op = '~'; // <> mean difference op ~
|
||||
else
|
||||
in.unget_token ( token, toktype );
|
||||
}
|
||||
else if ( op=='=' )
|
||||
{ toktype = in.get_token(token);
|
||||
if ( toktype==SrInput::Error ) return SrExpTable::translate_error ( in.last_error() );
|
||||
if ( token[0]!='=' ) return SrExpTable::MsgWrongEqualOperator;
|
||||
}
|
||||
else if ( op=='[' )
|
||||
{
|
||||
in.unget_token ( "(", SrInput::Delimiter ); // so that arrays indices may form an expression
|
||||
}
|
||||
setBinOp ( etok, op );
|
||||
SR_TRACE1 ( "Got a binary op: ["<<op<<"]");
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
if ( token[0]=='(' ) { setLeftPar(etok); SR_TRACE1("Got a left par.\n"); return SrExpTable::MsgOk; }
|
||||
if ( token[0]==')' ) { setRightPar(etok); SR_TRACE1("Got a right par.\n"); return SrExpTable::MsgOk; }
|
||||
if ( token[0]==']' ) { setRightPar(etok); SR_TRACE1("Got array right par.\n"); return SrExpTable::MsgOk; }
|
||||
|
||||
if ( toktype==SrInput::Name ) // bool(int), func or var
|
||||
{ if ( strcmp(token,"true")==0 ) { setNumb(etok,1.0); SR_TRACE1("Got a true.\n"); return SrExpTable::MsgOk; }
|
||||
if ( strcmp(token,"false")==0 ) { setNumb(etok,0.0); SR_TRACE1("Got a false.\n"); return SrExpTable::MsgOk; }
|
||||
|
||||
srFunc f = func_code ( token ); // found a func?
|
||||
if ( f!=srFuncUndefined )
|
||||
{ setFunc(etok,f); SR_TRACE1("Got func ["<<(Functions[(int)f])<<"]"); return SrExpTable::MsgOk; }
|
||||
|
||||
if ( !varman ) return SrExpTable::MsgUndefinedName; // found a name that is not a function or variable
|
||||
setVar ( etok, varman->get_variable_index(token) ); // get the user index for the variable
|
||||
SR_TRACE1("Got a var ["<<token<<"], index "<<etok.data.index );
|
||||
if ( etok.data.index<0 ) return SrExpTable::MsgUndefinedName;
|
||||
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
if ( toktype==SrInput::Integer || toktype==SrInput::Real )
|
||||
{ setNumb(etok,(double)atof(token)); SR_TRACE1("Got a number: "<<(atof(token))); return SrExpTable::MsgOk; }
|
||||
|
||||
return SrExpTable::MsgUndefChar;
|
||||
}
|
||||
|
||||
//===================================== SrExpTable ===================================================
|
||||
|
||||
SrExpTable::SrExpTable ()
|
||||
{
|
||||
_varman = 0;
|
||||
_delimiters = 0;
|
||||
}
|
||||
|
||||
SrExpTable::~SrExpTable ()
|
||||
{
|
||||
if ( _varman ) _varman->unref();
|
||||
delete _delimiters;
|
||||
}
|
||||
|
||||
void SrExpTable::set_var_manager ( SrExpTableVarManager* var_man )
|
||||
{
|
||||
if ( _varman ) _varman->unref();
|
||||
_varman = var_man;
|
||||
_varman->ref();
|
||||
}
|
||||
|
||||
void SrExpTable::set_parser_delimiters ( const char *delimiters )
|
||||
{
|
||||
sr_string_set ( _delimiters, delimiters );
|
||||
}
|
||||
|
||||
SrExpTable::Msg SrExpTable::parse ( SrInput& in, int &curline )
|
||||
{
|
||||
Msg msg;
|
||||
Token etok, t;
|
||||
srParMode mode = srParModeUnary;
|
||||
|
||||
_table.size ( 0 );
|
||||
_stack.size ( 0 );
|
||||
|
||||
while ( true )
|
||||
{
|
||||
msg = get_translated_token ( in, curline, etok, mode, _delimiters, _varman );
|
||||
if ( msg==MsgUnexpectedEndOfFile ) break;
|
||||
if ( msg!=MsgOk ) return msg;
|
||||
|
||||
if ( mode==srParModeUnary && (etok.type==Token::BinOp||etok.type==Token::RightPar) )
|
||||
{ return MsgOperandExpected; }
|
||||
|
||||
if ( mode==srParModeBinary && etok.type!=Token::BinOp && etok.type!=Token::RightPar )
|
||||
{ return MsgOperatorExpected; }
|
||||
|
||||
if ( mode==srParModeFunction && etok.type!=Token::LeftPar )
|
||||
{ return MsgFuncWithoutPar; }
|
||||
|
||||
switch ( etok.type )
|
||||
{ case Token::Numb :
|
||||
case Token::Var : _stack.push(etok); mode=srParModeBinary; break;
|
||||
case Token::LeftPar :
|
||||
case Token::UnaOp : _stack.push(etok); mode=srParModeUnary; break;
|
||||
case Token::Func : _stack.push(etok); mode=srParModeFunction; break;
|
||||
|
||||
case Token::BinOp :
|
||||
while ( !_stack.empty() )
|
||||
{ t=_stack.top ();
|
||||
if ( t.type==Token::BinOp && oprank(etok.data.code)>oprank(t.data.code) ) break;
|
||||
if ( t.type==Token::LeftPar ) break;
|
||||
_stack.pop(); _table.push(t);
|
||||
}
|
||||
_stack.push(etok); mode=srParModeUnary; break;
|
||||
|
||||
case Token::RightPar :
|
||||
if ( _stack.empty() ) return MsgStackEmpty;
|
||||
while ( !_stack.empty() )
|
||||
{ t=_stack.pop();
|
||||
if (t.type==Token::LeftPar) break;
|
||||
_table.push(t);
|
||||
}
|
||||
if ( t.type!=Token::LeftPar ) return MsgOk; // here the stack will be empty
|
||||
if ( !_stack.empty() && _stack.top().type==Token::Func )
|
||||
_table.push( _stack.pop() );
|
||||
mode=srParModeBinary;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if ( mode==srParModeUnary) return MsgOperandExpected;
|
||||
|
||||
while ( !_stack.empty() )
|
||||
{ if ( _stack.top().type==Token::LeftPar ) { return MsgUnmatchedParentesis; }
|
||||
_table.push( _stack.pop() );
|
||||
}
|
||||
|
||||
_table.push().type=Token::EndMark; // mark the end
|
||||
|
||||
# ifdef SR_USE_TRACE3
|
||||
sr_out << "ExpTable: " << *this << srnl;
|
||||
# endif
|
||||
|
||||
return MsgOk;
|
||||
}
|
||||
|
||||
static SrExpTable::Msg operateBinOp ( double x, double y, char op,
|
||||
SrArray<EToken>& stack,
|
||||
SrExpTableVarManager* varman )
|
||||
{
|
||||
double r;
|
||||
switch ( op )
|
||||
{ case '[' : r = varman->get_variable_value(int(x),int(y)); break;
|
||||
case '+' : r = x+y; break;
|
||||
case '-' : r = x-y; break;
|
||||
case '/' : if (!y) return SrExpTable::MsgDivideByZero; r=x/y; break;
|
||||
case '|' : if (!y) return SrExpTable::MsgDivideByZero; r=(double)(((int)x)|((int)y)); break;
|
||||
case '*' : r=x*y; break;
|
||||
case '%' : r = (x/100.0)*y; break;
|
||||
case '^' : if ( x==0.0 && y<0.0 ) return SrExpTable::MsgRaisedZeroToNeg;
|
||||
if ( x<0.0 && y!=(int)y ) return SrExpTable::MsgRaisedNegToReal;
|
||||
r=pow(x,y); break;
|
||||
case 'a' : r = x!=0 && y!=0? 1.0:0.0; break;
|
||||
case 'o' : r = x!=0 || y!=0? 1.0:0.0; break;
|
||||
case '>' : r = x>y? 1.0:0.0; break;
|
||||
case '<' : r = x<y? 1.0:0.0; break;
|
||||
case ')' : r = x>=y? 1.0:0.0; break;
|
||||
case '(' : r = x<=y? 1.0:0.0; break;
|
||||
case '~' : r = x!=y? 1.0:0.0; break;
|
||||
case '=' : r = x==y? 1.0:0.0; break;
|
||||
default : return SrExpTable::MsgUndefBinOp;
|
||||
}
|
||||
SR_TRACE2 ( "Operating: "<< x << " " << op << " " << y << " = " << r );
|
||||
setNumb ( stack.push(), r );
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
static SrExpTable::Msg operateUnaOp ( double x, char op, SrArray<EToken>& stack )
|
||||
{
|
||||
double r;
|
||||
switch ( op )
|
||||
{ case '+' : r=x; break;
|
||||
case '-' : r=-x; break;
|
||||
case '!' : r = x==0? 1.0:0.0; break;
|
||||
default : return SrExpTable::MsgUndefUnaOp;
|
||||
}
|
||||
SR_TRACE2 ( "Operating: " << op << " " << x << " = " << r );
|
||||
|
||||
setNumb ( stack.push(), r );
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
static double dround ( double x ) { return (x>0.0)? (double)(long int)(x+0.5) : (double)(long int)(x-0.5); }
|
||||
static double dsign ( double x ) { return (x<0.0)? -1: (x>0.0)? 1:0; }
|
||||
static double dfact ( int x ) { if(x<2)return 1l; long m=(long)x; while(--x>1)m*=(long)x; return(double)m; }
|
||||
|
||||
static SrExpTable::Msg operateFunc ( double x, srFunc f, SrArray<EToken>& stack )
|
||||
{
|
||||
double r;
|
||||
switch ( f )
|
||||
{ case srFuncAbs : r = x<0.0? -x:x; break;
|
||||
case srFuncAcos : if ( x<-1.0 || x>1.0 ) return SrExpTable::MsgAcosArgsOutOfRange;
|
||||
r=acos(x); break;
|
||||
case srFuncAsin : if ( x<-1.0 || x>1.0 ) return SrExpTable::MsgAsinArgsOutOfRange;
|
||||
r=asin(x); break;
|
||||
case srFuncAtan : r=atan(x); break;
|
||||
case srFuncCeil : r=ceil(x); break;
|
||||
case srFuncCos : r=cos(x); break;
|
||||
case srFuncExp : r=pow(SR_E,x); break;
|
||||
case srFuncFact : r=dfact((int)x); break;
|
||||
case srFuncFloor : r=floor(x); break;
|
||||
case srFuncLn : r=log(x); break;
|
||||
case srFuncLog : r=log10(x); break;
|
||||
case srFuncSign : r=dsign(x); break;
|
||||
case srFuncSin : r=sin(x); break;
|
||||
case srFuncSqrt : if ( x<-0.0 ) return SrExpTable::MsgSqrtArgIsNeg;
|
||||
r=sqrt(x); break;
|
||||
case srFuncTan : r=tan(x); break;
|
||||
case srFuncRound : r=dround(x); break;
|
||||
default : return SrExpTable::MsgUndefPreDefFunc;
|
||||
}
|
||||
|
||||
SR_TRACE2 ( "Operating: " << Functions[f] << "(" << x << ") = " << r );
|
||||
|
||||
setNumb ( stack.push(), r );
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
|
||||
static SrExpTable::Msg pop_value ( double &v, SrArray<EToken>& stack,
|
||||
SrExpTableVarManager* varman, bool isarrayop )
|
||||
{
|
||||
if ( stack.empty() ) return SrExpTable::MsgStackEmpty;
|
||||
EToken t = stack.pop();
|
||||
|
||||
if ( t.type==EToken::Var )
|
||||
{ v = isarrayop? (double)t.data.index : varman->get_variable_value(t.data.index,-1);
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
else if ( t.type==EToken::Numb )
|
||||
{ v = t.data.realval;
|
||||
return SrExpTable::MsgOk;
|
||||
}
|
||||
else
|
||||
return SrExpTable::MsgOperandExpected;
|
||||
}
|
||||
|
||||
SrExpTable::Msg SrExpTable::evaluate ( double &result )
|
||||
{
|
||||
int i;
|
||||
double u, v;
|
||||
Token::Type t;
|
||||
Msg msg;
|
||||
|
||||
_stack.size ( 0 );
|
||||
|
||||
for ( i=0; i<_table.size(); i++ )
|
||||
{ t = (Token::Type) _table[i].type;
|
||||
if ( t==Token::EndMark ) break; // Found a mark for the end (only when not compressed)
|
||||
|
||||
if ( t==Token::Numb || t==Token::Var ) // a value
|
||||
{ _stack.push ( _table[i] );
|
||||
}
|
||||
else
|
||||
{ msg = pop_value ( v, _stack, _varman, false );
|
||||
if ( msg!=MsgOk ) return msg;
|
||||
|
||||
switch ( t )
|
||||
{ case Token::Func :
|
||||
msg = operateFunc ( v, (srFunc)_table[i].data.index, _stack );
|
||||
break;
|
||||
|
||||
case Token::UnaOp :
|
||||
msg = operateUnaOp ( v, _table[i].data.code, _stack );
|
||||
break;
|
||||
|
||||
case Token::BinOp :
|
||||
msg = pop_value ( u, _stack, _varman, _table[i].data.code=='['?true:false );
|
||||
if ( msg!=MsgOk ) break;
|
||||
msg = operateBinOp ( u, v, _table[i].data.code, _stack, _varman );
|
||||
break;
|
||||
}
|
||||
|
||||
if ( msg!=MsgOk ) return msg;
|
||||
}
|
||||
}
|
||||
|
||||
if ( _stack.size()!=1 ) return MsgOperatorExpected;
|
||||
|
||||
return pop_value ( result, _stack, _varman, false );
|
||||
}
|
||||
|
||||
void SrExpTable::compress ()
|
||||
{
|
||||
_table.compress ();
|
||||
_stack.compress();
|
||||
}
|
||||
|
||||
/*! Takes the internal data of t, and leave t as an empty table. */
|
||||
// void take_data ( SrExpTable& t );
|
||||
|
||||
// void take_data ( SrArray<SrExpTableToken>& tokens );
|
||||
// void leave_data ( SrArray<SrExpTableToken>& tokens );
|
||||
|
||||
/*
|
||||
void SrExpTable::take_data ( SrExpTable& t )
|
||||
{
|
||||
if ( _tokens ) free ( _tokens );
|
||||
_tokens = t._tokens; t._tokens = 0;
|
||||
_size = t._size; t._size = 0;
|
||||
_capacity = t._capacity; t._capacity = 0;
|
||||
}
|
||||
*/
|
||||
|
||||
//=============================== friends ============================
|
||||
|
||||
SrOutput& operator << ( SrOutput& o, const SrExpTable& t )
|
||||
{
|
||||
EToken tk;
|
||||
|
||||
for ( int i=0; i<t._table.size(); i++ )
|
||||
{ tk = t._table[i];
|
||||
|
||||
switch ( tk.type )
|
||||
{ case EToken::Numb : o<<tk.data.realval<<srspc; break;
|
||||
case EToken::Var : o<<"var:"<<tk.data.index<<srspc; break;
|
||||
case EToken::Func : o<<Functions[tk.data.index]<<srspc; break;
|
||||
case EToken::UnaOp :
|
||||
case EToken::BinOp : o<<tk.data.code<<srspc; break;
|
||||
case EToken::EndMark : return o;
|
||||
}
|
||||
}
|
||||
|
||||
return o;
|
||||
|
||||
}
|
||||
|
||||
//============================ End of File ==============================
|
||||
|
||||
134
source/dcdt/se/sr_exp_table.h
Normal file
134
source/dcdt/se/sr_exp_table.h
Normal file
@ -0,0 +1,134 @@
|
||||
|
||||
/** \file sr_exp_table.h
|
||||
* Expression Table Evaluation. */
|
||||
|
||||
# ifndef SR_EXP_TABLE_H
|
||||
# define SR_EXP_TABLE_H
|
||||
|
||||
# include "sr_array.h"
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
# include "sr_shared_class.h"
|
||||
|
||||
/*! \class SrExpTableVarManager sr_exp_table.h
|
||||
\brief Allows the use of variables in expressions
|
||||
|
||||
This class should be derived and its two virtual methods
|
||||
implemented in order to make SrExpTable working with user-
|
||||
managed variables. Variables can be indexed like "var[n]". */
|
||||
class SrExpTableVarManager : public SrSharedClass
|
||||
{ public :
|
||||
|
||||
/*! virtual destructor will call the destructor of the derived class, if any. */
|
||||
virtual ~SrExpTableVarManager () {};
|
||||
|
||||
/*! Translates found user-variables into unique indices to be internally stored.
|
||||
Whenever a name is encountered during parsing, it is first checked if it
|
||||
corresponds to some internal function, if not, it is considered to be a
|
||||
user-variable. If -1 is returned, the variable is not defined and an
|
||||
error is generated. */
|
||||
virtual int get_variable_index ( const char* var_name )=0;
|
||||
|
||||
/*! Used to retrieve a variable value during evaluation. The
|
||||
parameter var_id is the variable index registered at parsing time, and
|
||||
if the parameter array_id is >=0, array_id will contain the array index
|
||||
requested for the variable. */
|
||||
virtual double get_variable_value ( int var_id, int array_id )=0;
|
||||
};
|
||||
|
||||
|
||||
/*! \class SrExpTable sr_exp_table.h
|
||||
\brief Expression Table Evaluation
|
||||
|
||||
SrExpTable is used to parse and keep an expression table for later evaluation.
|
||||
Recognized operators and their precedence are:
|
||||
(1) !x (logical unary operator that negates a value 0<->1)
|
||||
(2) x^y, x%y (x raised to y, x percent of y)
|
||||
(3) x*y, x/y, x|y (x multiplied by y, x divided by y, x modulus y)
|
||||
(4) x+y, x-y (x plus y, x minus y)
|
||||
(5) x>y, x<y, x>=y, x<=y, x==y, x<>y (comparison logical operators)
|
||||
(6) x and y, x or y (logical operators)
|
||||
Names true and false are converted to 1 and 0.
|
||||
All calculations are done using double types.
|
||||
Suported functions are: abs, acos, asin, atan, ceil, cos, exp, fact, floor,
|
||||
ln, log, round, sign, sin, sqrt, tan */
|
||||
class SrExpTable
|
||||
{ public :
|
||||
|
||||
enum Msg { MsgOk, MsgUndefChar, MsgNameTooBig,
|
||||
MsgStringNotClosed, MsgMisplacedDecPoint, MsgUnexpectedEndOfFile,
|
||||
MsgUndefinedName, MsgUndefBinOp, MsgUndefUnaOp,
|
||||
MsgUndefPreDefFunc, MsgOperandExpected, MsgOperatorExpected,
|
||||
MsgFuncWithoutPar, MsgDivideByZero, MsgRaisedZeroToNeg,
|
||||
MsgRaisedNegToReal, MsgUnmatchedParentesis, MsgAcosArgsOutOfRange,
|
||||
MsgAsinArgsOutOfRange, MsgStackEmpty, MsgSqrtArgIsNeg,
|
||||
MsgWrongEqualOperator
|
||||
};
|
||||
|
||||
|
||||
struct Token
|
||||
{ enum Type { Numb, Var, Func, BinOp, UnaOp, LeftPar, RightPar, EndMark };
|
||||
char type;
|
||||
union { double realval; int index; char code; } data;
|
||||
};
|
||||
|
||||
private :
|
||||
|
||||
SrArray<Token> _stack;
|
||||
SrArray<Token> _table;
|
||||
SrExpTableVarManager* _varman;
|
||||
char* _delimiters;
|
||||
|
||||
public : // static methods :
|
||||
|
||||
/*! This method can be used to get the name of all supported pre-defined
|
||||
functions. It returns the name of a given function index, that starts
|
||||
from zero. When index is out of range, null is returned. */
|
||||
static const char* function_name ( int index );
|
||||
|
||||
/*! This method returns the corresponding SrExpTable::Msg from the given
|
||||
SrInput::Error, the returned Msg can be one of : MsgOk, MsgUndefChar,
|
||||
MsgNameTooBig, MsgStringNotClosed, MsgMisplacedDecPoint. */
|
||||
static Msg translate_error ( SrInput::ErrorType e );
|
||||
|
||||
/*! Returns a string description of the given msg. Descriptions start with
|
||||
an upper case letter, and have no ending period. */
|
||||
static const char* msg_desc ( Msg m );
|
||||
|
||||
public :
|
||||
|
||||
/*! Constructor. */
|
||||
SrExpTable ();
|
||||
|
||||
/*! Destructor. */
|
||||
~SrExpTable ();
|
||||
|
||||
/*! A derived class of SrExpTableVarManager is needed in order to enable
|
||||
the use of variables. The passed manager must be allocated with operator
|
||||
new and its deletion is controlled by SrExpTable. A null pointer can
|
||||
be passed to indicate that variables are not used. */
|
||||
void set_var_manager ( SrExpTableVarManager* var_man );
|
||||
|
||||
/*! Make the parsing stop when any special delimeter is found.
|
||||
The parsing stops when one of the
|
||||
specified delimiters is read, and then,
|
||||
after ungetting the read delimiter, MsgOk is returned when parse() is called.
|
||||
If the passed string is null or "", delimiters will not be taken into account. */
|
||||
void set_parser_delimiters ( const char *delimiters );
|
||||
|
||||
/*! Parses an expression from the given input. The current line of the
|
||||
parsed input is updated in curline. */
|
||||
Msg parse ( SrInput &in, int &curline );
|
||||
|
||||
/*! Evaluates the expression and put the value in result if no error occured. */
|
||||
Msg evaluate ( double &result );
|
||||
|
||||
/*! Compress the internal arrays. */
|
||||
void compress ();
|
||||
|
||||
/*! Print the internal table. */
|
||||
friend SrOutput& operator << ( SrOutput& o, const SrExpTable& t );
|
||||
};
|
||||
|
||||
|
||||
# endif // SR_EXP_TABLE_H
|
||||
54
source/dcdt/se/sr_fl.h
Normal file
54
source/dcdt/se/sr_fl.h
Normal file
@ -0,0 +1,54 @@
|
||||
|
||||
/** \file sr_fl.h
|
||||
* fltk utilities integrated with sr
|
||||
*/
|
||||
|
||||
# ifndef SR_FL_H
|
||||
# define SR_FL_H
|
||||
|
||||
# include <FL/Fl_Gl_Window.H>
|
||||
# include <SR/sr_color.h>
|
||||
# include <SR/sr_string_array.h>
|
||||
# include <SR/sr_mat.h>
|
||||
|
||||
/*! Fltk color chooser utility receiving a sr parameter SrColor */
|
||||
bool fl_color_chooser ( const char *title, SrColor& c );
|
||||
|
||||
/*! Returns true if ok was pressed and false otherwise. Parameter value
|
||||
is displayed and returns the chosen value*/
|
||||
bool fl_value_input ( const char* title, const char* label, float& value );
|
||||
|
||||
/*! Returns true if ok was pressed and false otherwise. Parameter s
|
||||
is displayed for edition and returns the entered string */
|
||||
bool fl_string_input ( const char* title, const char* label, SrString& s );
|
||||
|
||||
/*! Uses an internal instance of SrOutputWindow to display a text.
|
||||
The internal instance is only allocated when used for the first time. */
|
||||
void fl_text_output ( const char* title, const char* text );
|
||||
|
||||
/*! Opens a matrix edition window, returning true if ok was pressed,
|
||||
and false otherwise. If title is null, the window title is not changed. */
|
||||
bool fl_matrix_input ( const char* title, SrMat& m );
|
||||
|
||||
/*! Scan the directory dir, and put all found directories and files in
|
||||
dirs and files. String array ext may contain file extensions to
|
||||
filter the read files (e.g "cpp" "txt", etc).
|
||||
String dir must finish with a slash '/' character.
|
||||
File names are returned in their full path names */
|
||||
void fl_scan_dir ( const SrString& dir,
|
||||
SrStringArray& dirs,
|
||||
SrStringArray& files,
|
||||
const SrStringArray& ext );
|
||||
|
||||
/*! Recursively scans and put in file all files starting at basedir.
|
||||
String array ext may contain file extensions to filter the read
|
||||
files; extensions are set without the "dot".
|
||||
File names are returned in their full path names */
|
||||
void fl_scan_files ( const SrString& basedir,
|
||||
SrStringArray& files,
|
||||
const SrStringArray& ext );
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_VIEWER_H
|
||||
|
||||
262
source/dcdt/se/sr_geo2.cpp
Normal file
262
source/dcdt/se/sr_geo2.cpp
Normal file
@ -0,0 +1,262 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdio.h>
|
||||
# include <math.h>
|
||||
|
||||
# include "sr_geo2.h"
|
||||
|
||||
// This file is designed to be as most as possible independent of other sr types
|
||||
|
||||
//================================= Macros =========================================
|
||||
|
||||
# define gABS(x) (x>0? (x):-(x))
|
||||
# define gMAX(a,b) (a>b? (a):(b))
|
||||
# define gSWAP(a,b) { tmp=a; a=b; b=tmp; }
|
||||
# define gOUTXY(a,b) printf("(%+6.4f,%+6.4f) ", a, b )
|
||||
# define gOUTL printf("\n");
|
||||
# define gEPS 1.0E-14 // doubles have 15 decimals
|
||||
# define gNEXTZERO(a) ( (a)>-(gEPS) && (a)<(gEPS) )
|
||||
|
||||
// The following macro can be defined to activate some extra operations that
|
||||
// try to get more precise results in the functions of this file. Normally
|
||||
// these extra operations are not worth to activate
|
||||
// # define gMAXPREC
|
||||
|
||||
//================================= funcs =====================================
|
||||
|
||||
bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y )
|
||||
{
|
||||
double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x);
|
||||
if ( gNEXTZERO(d) ) return false; // they are parallel
|
||||
double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d;
|
||||
if ( t<0.0 || t>1.0 ) return false; // outside [p1,p2]
|
||||
double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d;
|
||||
if ( s<0.0 || s>1.0 ) return false; // outside [p3,p4]
|
||||
return true;
|
||||
}
|
||||
|
||||
bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y,
|
||||
double& x, double &y )
|
||||
{
|
||||
double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x);
|
||||
if ( gNEXTZERO(d) ) return false; // they are parallel
|
||||
double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d;
|
||||
if ( t<0.0 || t>1.0 ) return false; // outside [p1,p2]
|
||||
double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d;
|
||||
if ( s<0.0 || s>1.0 ) return false; // outside [p3,p4]
|
||||
x = t*p1x+(1-t)*p2x;
|
||||
y = t*p1y+(1-t)*p2y;
|
||||
# ifdef gMAXPREC
|
||||
x += s*p3x+(1-s)*p4x;
|
||||
y += s*p3y+(1-s)*p4y;
|
||||
x /= 2;
|
||||
y /= 2;
|
||||
# endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y )
|
||||
{
|
||||
double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x);
|
||||
if ( gNEXTZERO(d) ) return false; // they are parallel
|
||||
return true;
|
||||
}
|
||||
|
||||
bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y,
|
||||
double& x, double &y )
|
||||
{
|
||||
double d = (p4y-p3y)*(p1x-p2x)-(p1y-p2y)*(p4x-p3x);
|
||||
if ( gNEXTZERO(d) ) return false; // they are parallel
|
||||
double t = ((p4y-p3y)*(p4x-p2x)-(p4x-p3x)*(p4y-p2y)) / d;
|
||||
x = t*p1x+(1-t)*p2x;
|
||||
y = t*p1y+(1-t)*p2y;
|
||||
# ifdef gMAXPREC // 1000 random tests reveal E-10 order error between t and s results
|
||||
double s = ((p4y-p2y)*(p1x-p2x)-(p1y-p2y)*(p4x-p2x)) / d;
|
||||
x += s*p3x+(1-s)*p4x;
|
||||
y += s*p3y+(1-s)*p4y;
|
||||
x /= 2;
|
||||
y /= 2;
|
||||
# endif
|
||||
return true;
|
||||
}
|
||||
|
||||
void sr_line_projection ( double p1x, double p1y, double p2x, double p2y, double px, double py, double& qx, double& qy )
|
||||
{
|
||||
qx = px;
|
||||
qy = py;
|
||||
double vx = -(p2y-p1y);
|
||||
double vy = p2x-p1x; // v = (p2-p1).ortho(), ortho==(-y,x)
|
||||
sr_lines_intersect ( p1x, p1y, p2x, p2y, px, py, px+vx, py+vy, qx, qy );
|
||||
}
|
||||
|
||||
bool sr_segment_projection ( double p1x, double p1y, double p2x, double p2y, double px, double py, double& qx, double& qy, double epsilon )
|
||||
{
|
||||
double tmp;
|
||||
sr_line_projection ( p1x, p1y, p2x, p2y, px, py, qx, qy );
|
||||
if ( epsilon==0 ) epsilon=gEPS; // if 0 the inequalities dont work with collinear inputs
|
||||
|
||||
/* if ( p1x>p2x ) gSWAP(p1x,p2x);
|
||||
if ( qx+epsilon<=p1x || qx-epsilon>=p2x ) return false;
|
||||
if ( p1y>p2y ) gSWAP(p1y,p2y);
|
||||
if ( qy+epsilon<=p1y || qy-epsilon>=p2y ) return false;*/
|
||||
|
||||
if ( p1x>p2x ) gSWAP(p1x,p2x);
|
||||
if ( qx+epsilon<p1x || qx-epsilon>p2x ) return false;
|
||||
if ( p1y>p2y ) gSWAP(p1y,p2y);
|
||||
if ( qy+epsilon<p1y || qy-epsilon>p2y ) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
double sr_dist2 ( double p1x, double p1y, double p2x, double p2y )
|
||||
{
|
||||
p1x = p2x-p1x;
|
||||
p1y = p2y-p1y;
|
||||
return p1x*p1x + p1y*p1y;
|
||||
}
|
||||
|
||||
double sr_point_segment_dist ( double px, double py, double p1x, double p1y, double p2x, double p2y )
|
||||
{
|
||||
double dist, qx, qy;
|
||||
|
||||
if ( sr_segment_projection(p1x,p1y,p2x,p2y,px,py,qx,qy,0) )
|
||||
{ dist = sr_dist2(px,py,qx,qy);
|
||||
}
|
||||
else
|
||||
{ dist = sr_dist2(px,py,p1x,p1y);
|
||||
double d = sr_dist2(px,py,p2x,p2y);
|
||||
if (d<dist) dist=d;
|
||||
}
|
||||
|
||||
return sqrt(dist);
|
||||
}
|
||||
|
||||
double sr_point_line_dist ( double px, double py, double p1x, double p1y, double p2x, double p2y )
|
||||
{
|
||||
double qx, qy;
|
||||
sr_line_projection (p1x,p1y,p2x,p2y,px,py,qx,qy);
|
||||
return sqrt( sr_dist2(px,py,qx,qy) );
|
||||
}
|
||||
|
||||
bool sr_next ( double p1x, double p1y, double p2x, double p2y, double epsilon )
|
||||
{
|
||||
return sr_dist2(p1x,p1y,p2x,p2y)<=epsilon*epsilon? true:false;
|
||||
}
|
||||
|
||||
double sr_ccw ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y )
|
||||
{
|
||||
return SR_CCW(p1x,p1y,p2x,p2y,p3x,p3y);
|
||||
}
|
||||
|
||||
bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y, double px, double py, double epsilon )
|
||||
{
|
||||
double qx, qy;
|
||||
if ( epsilon==0 ) epsilon=gEPS;
|
||||
if ( !sr_segment_projection ( p1x, p1y, p2x, p2y, px, py, qx, qy, epsilon ) ) return false;
|
||||
return sr_dist2(px,py,qx,qy)<=epsilon*epsilon? true:false;
|
||||
}
|
||||
|
||||
bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y, double px, double py,
|
||||
double epsilon, double& dist2 )
|
||||
{
|
||||
double qx, qy;
|
||||
if ( epsilon==0 ) epsilon=gEPS;
|
||||
if ( !sr_segment_projection ( p1x, p1y, p2x, p2y, px, py, qx, qy, epsilon ) ) return false;
|
||||
dist2 = sr_dist2(px,py,qx,qy);
|
||||
//sr_out<<dist2<<srspc<<(epsilon*epsilon)<<srnl;
|
||||
return dist2<=epsilon*epsilon? true:false;
|
||||
}
|
||||
|
||||
bool sr_in_triangle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double px, double py )
|
||||
{
|
||||
return SR_CCW(px,py,p1x,p1y,p2x,p2y)>=0 &&
|
||||
SR_CCW(px,py,p2x,p2y,p3x,p3y)>=0 &&
|
||||
SR_CCW(px,py,p3x,p3y,p1x,p1y)>=0 ? true:false;
|
||||
}
|
||||
|
||||
// circle_test :
|
||||
// p1, p2, p3 must be in ccw order
|
||||
// Calculates the following determinant :
|
||||
// | p1x p1y p1x*p1x + p1y*p1y 1.0 |
|
||||
// | p2x p2y p2x*p2x + p2y*p2y 1.0 |
|
||||
// | p3x p3y p3x*p3x + p3y*p3y 1.0 |
|
||||
// | px py px*px + py*py 1.0 |
|
||||
// This is not the most accurate calculation, but is the fastest.
|
||||
bool sr_in_circle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y, double px, double py )
|
||||
{
|
||||
double p1z = p1x*p1x + p1y*p1y;
|
||||
double p2z = p2x*p2x + p2y*p2y;
|
||||
double p3z = p3x*p3x + p3y*p3y;
|
||||
double pz = px*px + py*py;
|
||||
|
||||
double m12 = p2x*p3y - p2y*p3x;
|
||||
double m13 = p2x*p3z - p2z*p3x;
|
||||
double m14 = p2x - p3x;
|
||||
double m23 = p2y*p3z - p2z*p3y;
|
||||
double m24 = p2y - p3y;
|
||||
double m34 = p2z - p3z;
|
||||
double d1 = p1y*m34 - p1z*m24 + m23;
|
||||
double d2 = p1x*m34 - p1z*m14 + m13;
|
||||
double d3 = p1x*m24 - p1y*m14 + m12;
|
||||
double d4 = p1x*m23 - p1y*m13 + p1z*m12;
|
||||
|
||||
double det = d4 - px*d1 + py*d2 - pz*d3;
|
||||
|
||||
const double prec = gEPS; // I have encountered cases working only with this tiny epsilon
|
||||
return det>prec? true:false;
|
||||
}
|
||||
|
||||
void sr_barycentric ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y,
|
||||
double px, double py, double& u, double& v, double& w )
|
||||
{
|
||||
# define DET3(a,b,c,d,e,f,g,h,i) a*e*i +b*f*g +d*h*c -c*e*g -b*d*i -a*f*h
|
||||
double A = DET3 ( p1x, p2x, p3x, p1y, p2y, p3y, 1, 1, 1 );
|
||||
double A1 = DET3 ( px, p2x, p3x, py, p2y, p3y, 1, 1, 1 );
|
||||
double A2 = DET3 ( p1x, px, p3x, p1y, py, p3y, 1, 1, 1 );
|
||||
//double A3 = DET3 ( p1x, p2x, px, p1y, p2y, py, 1, 1, 1 );
|
||||
# undef DET3
|
||||
u = A1/A;
|
||||
v = A2/A;
|
||||
w = 1.0-u-v; // == A3/A;
|
||||
}
|
||||
|
||||
//=============================== Documentation ======================================
|
||||
|
||||
/*
|
||||
Intersection of segments math:
|
||||
|
||||
t p1 + (1-t)p2 = p (1)
|
||||
s p3 + (1-s)p4 = p (2)
|
||||
|
||||
=> Making (1)=(2) :
|
||||
t p1 + (1-t)p2 = s p3 + (1-s)p4
|
||||
t(p1-p2) + s(p4-p3) = p4-p2
|
||||
t(p1x-p2x) + s(p4x-p3x) = p4x-p2x (3)
|
||||
t(p1y-p2y) + s(p4y-p3y) = p4y-p2y (4)
|
||||
|
||||
=> Putting t from (3) to (4) :
|
||||
t = [(p4x-p2x) - s(p4x-p3x)] / (p1x-p2x)
|
||||
[(p4x-p2x) - s(p4x-p3x)] / (p1x-p2x) = [(p4y-p2y) - s(p4y-p3y)] / (p1y-p2y)
|
||||
(p1y-p2y)(p4x-p2x) - s(p1y-p2y)(p4x-p3x) = (p4y-p2y)(p1x-p2x) - s(p4y-p3y)(p1x-p2x)
|
||||
s(p4y-p3y)(p1x-p2x) - s(p1y-p2y)(p4x-p3x) = (p4y-p2y)(p1x-p2x) - (p1y-p2y)(p4x-p2x)
|
||||
s = [(p4y-p2y)(p1x-p2x)-(p1y-p2y)(p4x-p2x)] / [(p4y-p3y)(p1x-p2x)-(p1y-p2y)(p4x-p3x)]
|
||||
Let d = (p4y-p3y)(p1x-p2x)-(p1y-p2y)(p4x-p3x) (5)
|
||||
s = [(p4y-p2y)(p1x-p2x)-(p1y-p2y)(p4x-p2x)] / d (6)
|
||||
|
||||
=> Putting s from (3) to (4) :
|
||||
s = [(p4x-p2x) - t(p1x-p2x)] / (p4x-p3x)
|
||||
[(p4x-p2x) - t(p1x-p2x)] / (p4x-p3x) = [(p4y-p2y) - t(p1y-p2y)] / (p4y-p3y)
|
||||
(p4y-p3y)(p4x-p2x) - t(p4y-p3y)(p1x-p2x) = (p4x-p3x)(p4y-p2y) - t(p4x-p3x)(p1y-p2y)
|
||||
t(p4x-p3x)(p1y-p2y) - t(p4y-p3y)(p1x-p2x) = (p4x-p3x)(p4y-p2y) - (p4y-p3y)(p4x-p2x)
|
||||
t = [(p4x-p3x)(p4y-p2y)-(p4y-p3y)(p4x-p2x)] / [(p4x-p3x)(p1y-p2y)-(p4y-p3y)(p1x-p2x)]
|
||||
t = -1*[(p4x-p3x)(p4y-p2y)-(p4y-p3y)(p4x-p2x)] / -1*[(p4x-p3x)(p1y-p2y)-(p4y-p3y)(p1x-p2x)]
|
||||
t = [(p4y-p3y)(p4x-p2x)-(p4x-p3x)(p4y-p2y)] / [(p4y-p3y)(p1x-p2x)-(p4x-p3x)(p1y-p2y)]
|
||||
Using (5) :
|
||||
t = [(p4y-p3y)(p4x-p2x)-(p4x-p3x)(p4y-p2y)] / d (7)
|
||||
|
||||
=> From (6) and (7), t and s determines p:
|
||||
p = t p1 + (1-t)p2 = s p3 + (1-s)p4 */
|
||||
|
||||
//=============================== End of File ======================================
|
||||
90
source/dcdt/se/sr_geo2.h
Normal file
90
source/dcdt/se/sr_geo2.h
Normal file
@ -0,0 +1,90 @@
|
||||
|
||||
/** \file sr_geo2.h
|
||||
* 2d geometric primitives */
|
||||
|
||||
# ifndef SR_GEO2_H
|
||||
# define SR_GEO2_H
|
||||
|
||||
# define SR_CCW(ax,ay,bx,by,cx,cy) ((ax*by)-(bx*ay)+(bx*cy)-(cx*by)+(cx*ay)-(ax*cy))
|
||||
|
||||
/*! Returns true if segments (p1,p2) and (p3,p4) intersect, and false otherwise. */
|
||||
bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y );
|
||||
|
||||
/*! Returns true if segments (p1,p2) and (p3,p4) intersect, and false otherwise.
|
||||
In case of intersection, p will be the intersection point. */
|
||||
bool sr_segments_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y,
|
||||
double& x, double &y );
|
||||
|
||||
/*! Returns true if lines (p1,p2) and (p3,p4) intersect, and false otherwise. */
|
||||
bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y );
|
||||
|
||||
/*! Returns true if lines (p1,p2) and (p3,p4) intersect, and false otherwise.
|
||||
In case of intersection, p will be the intersection point. */
|
||||
bool sr_lines_intersect ( double p1x, double p1y, double p2x, double p2y,
|
||||
double p3x, double p3y, double p4x, double p4y,
|
||||
double& x, double &y );
|
||||
|
||||
/*! Orthogonal projection of p in the line (p1,p2). The projected point becomes q. */
|
||||
void sr_line_projection ( double p1x, double p1y, double p2x, double p2y,
|
||||
double px, double py,
|
||||
double& qx, double& qy );
|
||||
|
||||
/*! Returns true if the orthogonal projection of p is inside (within epsilon distance)
|
||||
the segment (p1,p2). In such a case, the projected point becomes q, otherwise
|
||||
false is returned. */
|
||||
bool sr_segment_projection ( double p1x, double p1y, double p2x, double p2y,
|
||||
double px, double py,
|
||||
double& qx, double& qy, double epsilon );
|
||||
|
||||
/*! Returns the square of the distance between p1 and p2 */
|
||||
double sr_dist2 ( double p1x, double p1y, double p2x, double p2y );
|
||||
|
||||
/*! Returns the minimum distance between p and segment (p1,p2) */
|
||||
double sr_point_segment_dist ( double px, double py,
|
||||
double p1x, double p1y, double p2x, double p2y );
|
||||
|
||||
/*! Returns the minimum distance between p and line (p1,p2) */
|
||||
double sr_point_line_dist ( double px, double py, double p1x, double p1y, double p2x, double p2y );
|
||||
|
||||
/*! Returns true if the distance between p1 and p2 is smaller (or equal) than epsilon */
|
||||
bool sr_next ( double p1x, double p1y, double p2x, double p2y, double epsilon );
|
||||
|
||||
/*! Returns >0 if the three points are in counter-clockwise order, <0 if
|
||||
the order is clockwise and 0 if points are collinear. */
|
||||
double sr_ccw ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y );
|
||||
|
||||
/*! Returns true if p is in the segment (p1,p2), within precision epsilon, and false
|
||||
otherwise. More precisely, true is returned if the projection of p in the segment (p1,p2)
|
||||
has a distance to p less or equal than epsilon. */
|
||||
bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y,
|
||||
double px, double py, double epsilon );
|
||||
|
||||
/*! Returns true if p is in the segment (p1,p2), within precision epsilon, and false
|
||||
otherwise. More precisely, true is returned if the projection of p in the segment
|
||||
p1,p2) has a distance to p less or equal than epsilon. Parameter dist2 contains
|
||||
the square of that distance. */
|
||||
bool sr_in_segment ( double p1x, double p1y, double p2x, double p2y, double px, double py,
|
||||
double epsilon, double& dist2 );
|
||||
|
||||
/*! Returns true if p is inside (or in the border) of triangle (p1,p2,p3), otherwise false
|
||||
is returned. The test is based on 3 CCW>=0 tests and no epsilons are used. */
|
||||
bool sr_in_triangle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y,
|
||||
double px, double py );
|
||||
|
||||
/*! Returns true if p is inside the circle passing at p1, p2, and p3, otherwise false
|
||||
is returned. Points p1, p2 and p3 must be in ccw orientation.
|
||||
This is a fast 4x4 determinant evaluation that will return true only
|
||||
if p is strictly inside the circle, testing if determinant>1.0E-14 */
|
||||
bool sr_in_circle ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y,
|
||||
double px, double py );
|
||||
|
||||
/*! Returns (u,v,w), w==1-u-v, u+v+w==1, such that p1*u + p2*v + p3w == p */
|
||||
void sr_barycentric ( double p1x, double p1y, double p2x, double p2y, double p3x, double p3y,
|
||||
double px, double py, double& u, double& v, double& w );
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_GEO2_H
|
||||
73
source/dcdt/se/sr_gl.h
Normal file
73
source/dcdt/se/sr_gl.h
Normal file
@ -0,0 +1,73 @@
|
||||
|
||||
# ifndef SR_GL_H
|
||||
# define SR_GL_H
|
||||
|
||||
/** \file sr_gl.h
|
||||
* Sr wrapper and extensions for OpenGL
|
||||
*
|
||||
* Overload of most used OpenGL functions to directly work with SR types,
|
||||
* and some extra utilities.
|
||||
*/
|
||||
|
||||
# include <SR/sr.h>
|
||||
|
||||
# ifdef SR_TARGET_WINDOWS // defined in sr.h
|
||||
# include <Windows.h>
|
||||
# endif
|
||||
|
||||
# include <GL/gl.h>
|
||||
|
||||
class SrVec;
|
||||
class SrMat;
|
||||
class SrQuat;
|
||||
class SrColor;
|
||||
class SrLight;
|
||||
class SrImage;
|
||||
class SrOutput;
|
||||
class SrMaterial;
|
||||
|
||||
//======================================= geometry ================================
|
||||
|
||||
void glNormal ( const SrVec &v );
|
||||
|
||||
void glVertex ( const SrVec &v );
|
||||
void glVertex ( const SrVec &v1, const SrVec &v2 );
|
||||
void glVertex ( const SrVec &v1, const SrVec &v2, const SrVec &v3 );
|
||||
void glVertex ( const SrVec &v1, const SrVec &v2, const SrVec &v3, const SrVec &v4 );
|
||||
void glVertex ( float x, float y, float z );
|
||||
void glVertex ( float x, float y, float z, float a, float b, float c );
|
||||
void glVertex ( float x, float y );
|
||||
void glDrawBox ( const SrVec& a, const SrVec& b ); //!< Send quads with normals forming the box
|
||||
|
||||
//====================================== appearance ================================
|
||||
|
||||
void glClearColor ( const SrColor& c );
|
||||
void glColor ( const SrColor& c );
|
||||
void glLight ( int id, const SrLight& l ); //!< id = x E {0,...,7}, from GL_LIGHTx
|
||||
void glMaterial ( const SrMaterial &m ); //!< Sets material for GL_FRONT_AND_BACK
|
||||
void glMaterialFront ( const SrMaterial &m );
|
||||
void glMaterialBack ( const SrMaterial &m );
|
||||
|
||||
//==================================== matrices ==============================
|
||||
|
||||
void glMultMatrix ( const SrMat &m );
|
||||
void glLoadMatrix ( const SrMat &m );
|
||||
void glTranslate ( const SrVec &v );
|
||||
void glScale ( float s );
|
||||
void glRotate ( const SrQuat &q );
|
||||
void glLookAt ( const SrVec &eye, const SrVec ¢er, const SrVec &up );
|
||||
void glPerspective ( float fovy, float aspect, float znear, float zfar );
|
||||
void glGetViewMatrix ( SrMat &m );
|
||||
void glGetProjectionMatrix ( SrMat &m );
|
||||
|
||||
//==================================== extras ==============================
|
||||
|
||||
void glSnapshot ( SrImage& img );
|
||||
|
||||
//==================================== info ==============================
|
||||
|
||||
void glPrintInfo ( SrOutput &o );
|
||||
|
||||
//================================ End of File ==================================
|
||||
|
||||
# endif // SR_GL_H
|
||||
48
source/dcdt/se/sr_gl_render_funcs.h
Normal file
48
source/dcdt/se/sr_gl_render_funcs.h
Normal file
@ -0,0 +1,48 @@
|
||||
|
||||
# ifndef SR_GL_RENDER_FUNCS_H
|
||||
# define SR_GL_RENDER_FUNCS_H
|
||||
|
||||
/** \file sr_gl_render_funcs.h
|
||||
* OpenGL render functions of SR shapes
|
||||
*/
|
||||
|
||||
class SrSnShapeBase;
|
||||
|
||||
/*! All render functions used to render SR shapes are static methods
|
||||
of class SrGlRenderFuncs. They are automatically registered to
|
||||
the ogl render action. See SrGlRenderAction class description. */
|
||||
class SrGlRenderFuncs
|
||||
{ public:
|
||||
static void render_model ( SrSnShapeBase* shape );
|
||||
|
||||
static void render_lines ( SrSnShapeBase* shape );
|
||||
|
||||
static void render_points ( SrSnShapeBase* shape );
|
||||
|
||||
/*! The default render mode is smooth, and flat has the same
|
||||
effect of smooth shading. */
|
||||
static void render_box ( SrSnShapeBase* shape );
|
||||
|
||||
/*! The resolution value stored in SrSnShapeBase
|
||||
indicates how many triangles is used to
|
||||
draw the sphere. A value lower or equal to 0.2 defines
|
||||
the first level (8 triangles), then levels are increased
|
||||
each time 0.2 is added to the resolution. Resolution 1.0
|
||||
represents a well discretized sphere. */
|
||||
static void render_sphere ( SrSnShapeBase* shape );
|
||||
|
||||
/*! The resolution value stored in SrSnShapeBase
|
||||
represents 1/10th of the number of edges discretizing the base
|
||||
circle of the cylinder. For ex., resolution 1.0 results in
|
||||
10 edges, and gives 40 triangles to render the cylinder. */
|
||||
static void render_cylinder ( SrSnShapeBase* shape );
|
||||
|
||||
static void render_polygon ( SrSnShapeBase* shape );
|
||||
|
||||
static void render_polygons ( SrSnShapeBase* shape );
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_GL_RENDER_FUNCS_H
|
||||
|
||||
663
source/dcdt/se/sr_graph.cpp
Normal file
663
source/dcdt/se/sr_graph.cpp
Normal file
@ -0,0 +1,663 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdlib.h>
|
||||
# include "sr_graph.h"
|
||||
# include "sr_heap.h"
|
||||
|
||||
//============================== SrGraphLink ===============================================
|
||||
|
||||
|
||||
|
||||
|
||||
//============================== SrGraphNode ===============================================
|
||||
|
||||
SrGraphNode::~SrGraphNode ()
|
||||
{
|
||||
SrClassManagerBase* lman = _graph->link_class_manager();
|
||||
if ( _links.size()>0 )
|
||||
lman->free ( _links.pop() );
|
||||
}
|
||||
|
||||
SrGraphLink* SrGraphNode::linkto ( SrGraphNode* n, float cost )
|
||||
{
|
||||
SrGraphLink* l = (SrGraphLink*) _graph->link_class_manager()->alloc();
|
||||
l->_node = n;
|
||||
l->_index = 0;
|
||||
l->_cost = cost;
|
||||
|
||||
_links.push () = l;
|
||||
|
||||
return l;
|
||||
}
|
||||
|
||||
void SrGraphNode::unlink ( int ni )
|
||||
{
|
||||
SrClassManagerBase* lman = _graph->link_class_manager();
|
||||
lman->free ( _links[ni] );
|
||||
_links[ni] = _links.pop();
|
||||
}
|
||||
|
||||
int SrGraphNode::search_link ( SrGraphNode* n ) const
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<_links.size(); i++ )
|
||||
if ( _links[i]->node()==n ) return i;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void SrGraphNode::output ( SrOutput& o ) const
|
||||
{
|
||||
float cost;
|
||||
int i, max;
|
||||
max = _links.size()-1;
|
||||
if ( max<0 ) return;
|
||||
|
||||
SrClassManagerBase* lman = _graph->link_class_manager();
|
||||
|
||||
o<<'(';
|
||||
for ( i=0; i<=max; i++ )
|
||||
{ // output blocked status
|
||||
o << _links[i]->_blocked << srspc;
|
||||
|
||||
// output index to target node
|
||||
o << _links[i]->_node->_index << srspc;
|
||||
|
||||
// output link cost
|
||||
cost = _links[i]->_cost;
|
||||
if ( cost==SR_TRUNC(cost) ) o << int(cost);
|
||||
else o << cost;
|
||||
|
||||
// output user data
|
||||
o << srspc;
|
||||
lman->output(o,_links[i]);
|
||||
|
||||
if ( i<max ) o<<srspc;
|
||||
}
|
||||
o<<')';
|
||||
}
|
||||
|
||||
//============================== SrGraphPathTree ===============================================
|
||||
|
||||
class SrGraphPathTree
|
||||
{ public :
|
||||
struct Node { int parent; float cost; SrGraphNode* node; };
|
||||
struct Leaf { int l; int d; }; // the leaf index in nodes array, and its depth
|
||||
SrArray<Node> nodes;
|
||||
SrHeap<Leaf,float> leafs;
|
||||
SrGraphBase* graph;
|
||||
SrGraphNode* closest;
|
||||
int iclosest;
|
||||
float cdist;
|
||||
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata );
|
||||
void *udata;
|
||||
bool bidirectional_block;
|
||||
|
||||
public :
|
||||
SrGraphPathTree ()
|
||||
{ bidirectional_block = false;
|
||||
}
|
||||
|
||||
void init ( SrGraphBase* g, SrGraphNode* n )
|
||||
{ nodes.size(1);
|
||||
nodes[0].parent = -1;
|
||||
nodes[0].cost = 0;
|
||||
nodes[0].node = n;
|
||||
Leaf l;
|
||||
l.l = l.d = 0;
|
||||
leafs.insert ( l, 0 );
|
||||
graph = g;
|
||||
distfunc=0;
|
||||
udata = 0;
|
||||
closest = 0;
|
||||
iclosest = 0;
|
||||
cdist = 0;
|
||||
}
|
||||
|
||||
bool has_leaf ()
|
||||
{ return leafs.size()==0? false:true;
|
||||
}
|
||||
|
||||
bool expand_lowest_cost_leaf ( SrGraphNode* goalnode )
|
||||
{ int i;
|
||||
int n = leafs.top().l;
|
||||
int d = leafs.top().d;
|
||||
leafs.remove ();
|
||||
SrGraphNode* node = nodes[n].node;
|
||||
Leaf leaf;
|
||||
const SrArray<SrGraphLink*>& a = node->links();
|
||||
for ( i=0; i<a.size(); i++ )
|
||||
{ //sr_out<<a.size()<<srnl;
|
||||
if ( graph->marked(a[i]) ||
|
||||
a[i]->blocked() ||
|
||||
a[i]->node()->blocked() ) continue;
|
||||
if ( bidirectional_block )
|
||||
{ if ( a[i]->node()->link(node)->blocked() ) continue; }
|
||||
nodes.push();
|
||||
nodes.top().parent = n;
|
||||
nodes.top().cost = nodes[n].cost + a[i]->cost();
|
||||
nodes.top().node = a[i]->node();
|
||||
leaf.l = nodes.size()-1;
|
||||
leaf.d = d+1;
|
||||
leafs.insert ( leaf, nodes.top().cost );
|
||||
graph->mark ( a[i] );
|
||||
if ( distfunc )
|
||||
{ float d = distfunc ( nodes.top().node, goalnode, udata );
|
||||
if ( !closest || d<cdist )
|
||||
{ closest=nodes.top().node; iclosest=nodes.size()-1; cdist=d; }
|
||||
}
|
||||
if ( a[i]->node()==goalnode ) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
//============================== SrGraphBase ===============================================
|
||||
|
||||
# define MARKFREE 0
|
||||
# define MARKING 1
|
||||
# define INDEXING 2
|
||||
|
||||
SrGraphBase::SrGraphBase ( SrClassManagerBase* nm, SrClassManagerBase* lm )
|
||||
:_nodes(nm)
|
||||
{
|
||||
_curmark = 1;
|
||||
_mark_status = MARKFREE;
|
||||
_pt = 0; // allocated only if shortest path is called
|
||||
_lman = lm;
|
||||
_lman->ref(); // nm is managed by the list _nodes
|
||||
_leave_indices_after_save = 0;
|
||||
}
|
||||
|
||||
SrGraphBase::~SrGraphBase ()
|
||||
{
|
||||
_nodes.init (); // Important: this ensures that _lman is used before _lman->unref()
|
||||
delete _pt;
|
||||
_lman->unref();
|
||||
}
|
||||
|
||||
void SrGraphBase::init ()
|
||||
{
|
||||
_nodes.init();
|
||||
_curmark = 1;
|
||||
_mark_status = MARKFREE;
|
||||
}
|
||||
|
||||
void SrGraphBase::compress ()
|
||||
{
|
||||
if ( _nodes.empty() ) return;
|
||||
_nodes.gofirst();
|
||||
do { _nodes.cur()->compress();
|
||||
} while ( _nodes.notlast() );
|
||||
}
|
||||
|
||||
int SrGraphBase::num_links () const
|
||||
{
|
||||
if ( _nodes.empty() ) return 0;
|
||||
|
||||
int n=0;
|
||||
|
||||
SrGraphNode* first = _nodes.first();
|
||||
SrGraphNode* cur = first;
|
||||
do { n += cur->num_links();
|
||||
cur = cur->next();
|
||||
} while ( cur!=first );
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
//----------------------------------- marking --------------------------------
|
||||
|
||||
void SrGraphBase::begin_marking ()
|
||||
{
|
||||
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_mark() is locked!");
|
||||
_mark_status = MARKING;
|
||||
if ( _curmark==sruintmax ) _normalize_mark ();
|
||||
else _curmark++;
|
||||
}
|
||||
|
||||
void SrGraphBase::end_marking ()
|
||||
{
|
||||
_mark_status=MARKFREE;
|
||||
}
|
||||
|
||||
bool SrGraphBase::marked ( SrGraphNode* n )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(n): marking is not active!\n" );
|
||||
return n->_index==_curmark? true:false;
|
||||
}
|
||||
|
||||
void SrGraphBase::mark ( SrGraphNode* n )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(n): marking is not active!\n" );
|
||||
n->_index = _curmark;
|
||||
}
|
||||
|
||||
void SrGraphBase::unmark ( SrGraphNode* n )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(n): marking is not active!\n");
|
||||
n->_index = _curmark-1;
|
||||
}
|
||||
|
||||
bool SrGraphBase::marked ( SrGraphLink* l )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(l): marking is not active!\n" );
|
||||
return l->_index==_curmark? true:false;
|
||||
}
|
||||
|
||||
void SrGraphBase::mark ( SrGraphLink* l )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(l): marking is not active!\n" );
|
||||
l->_index = _curmark;
|
||||
}
|
||||
|
||||
void SrGraphBase::unmark ( SrGraphLink* l )
|
||||
{
|
||||
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(l): marking is not active!\n");
|
||||
l->_index = _curmark-1;
|
||||
}
|
||||
|
||||
//----------------------------------- indexing --------------------------------
|
||||
|
||||
void SrGraphBase::begin_indexing ()
|
||||
{
|
||||
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_indexing() is locked!");
|
||||
_mark_status = INDEXING;
|
||||
}
|
||||
|
||||
void SrGraphBase::end_indexing ()
|
||||
{
|
||||
_normalize_mark ();
|
||||
_mark_status=MARKFREE;
|
||||
}
|
||||
|
||||
sruint SrGraphBase::index ( SrGraphNode* n )
|
||||
{
|
||||
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n): indexing is not active!");
|
||||
return n->_index;
|
||||
}
|
||||
|
||||
void SrGraphBase::index ( SrGraphNode* n, sruint i )
|
||||
{
|
||||
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n,i): indexing is not active!");
|
||||
n->_index = i;
|
||||
}
|
||||
|
||||
sruint SrGraphBase::index ( SrGraphLink* l )
|
||||
{
|
||||
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l): indexing is not active!");
|
||||
return l->_index;
|
||||
}
|
||||
|
||||
void SrGraphBase::index ( SrGraphLink* l, sruint i )
|
||||
{
|
||||
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l,i): indexing is not active!");
|
||||
l->_index = i;
|
||||
}
|
||||
|
||||
//----------------------------------- construction --------------------------------
|
||||
|
||||
SrGraphNode* SrGraphBase::insert ( SrGraphNode* n )
|
||||
{
|
||||
_nodes.insert_next ( n );
|
||||
n->_graph = this;
|
||||
return n;
|
||||
}
|
||||
|
||||
SrGraphNode* SrGraphBase::extract ( SrGraphNode* n )
|
||||
{
|
||||
_nodes.cur(n);
|
||||
return _nodes.extract();
|
||||
}
|
||||
|
||||
void SrGraphBase::remove_node ( SrGraphNode* n )
|
||||
{
|
||||
_nodes.cur(n);
|
||||
_nodes.remove();
|
||||
}
|
||||
|
||||
int SrGraphBase::remove_link ( SrGraphNode* n1, SrGraphNode* n2 )
|
||||
{
|
||||
int i;
|
||||
int n=0;
|
||||
|
||||
while ( true )
|
||||
{ i = n1->search_link(n2);
|
||||
if ( i<0 ) break;
|
||||
n++;
|
||||
n1->unlink(i);
|
||||
}
|
||||
|
||||
while ( true )
|
||||
{ i = n2->search_link(n1);
|
||||
if ( i<0 ) break;
|
||||
n++;
|
||||
n2->unlink(i);
|
||||
}
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
void SrGraphBase::link ( SrGraphNode* n1, SrGraphNode* n2, float c )
|
||||
{
|
||||
n1->linkto(n2,c);
|
||||
n2->linkto(n1,c);
|
||||
}
|
||||
|
||||
//----------------------------------- get edges ----------------------------------
|
||||
|
||||
void SrGraphBase::get_directed_edges ( SrArray<SrGraphNode*>& edges )
|
||||
{
|
||||
edges.size ( num_nodes() );
|
||||
edges.size ( 0 );
|
||||
|
||||
int i;
|
||||
SrGraphNode* n;
|
||||
SrListIterator<SrGraphNode> it(_nodes);
|
||||
|
||||
for ( it.first(); it.inrange(); it.next() )
|
||||
{ n = it.get();
|
||||
for ( i=0; i<n->num_links(); i++ )
|
||||
{ edges.push() = n;
|
||||
edges.push() = n->link(i)->node();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrGraphBase::get_undirected_edges ( SrArray<SrGraphNode*>& edges )
|
||||
{
|
||||
edges.size ( num_nodes() );
|
||||
edges.size ( 0 );
|
||||
|
||||
int i, li;
|
||||
SrGraphNode* n;
|
||||
SrListIterator<SrGraphNode> it(_nodes);
|
||||
|
||||
begin_marking();
|
||||
|
||||
for ( it.first(); it.inrange(); it.next() )
|
||||
{ n = it.get();
|
||||
for ( i=0; i<n->links().size(); i++ )
|
||||
{ if ( !marked(n->link(i)) )
|
||||
{ edges.push() = n;
|
||||
edges.push() = n->link(i)->node();
|
||||
mark ( n->link(i) );
|
||||
li = edges.top()->search_link(n);
|
||||
if ( li>=0 ) mark ( edges.top()->link(li) );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
end_marking();
|
||||
}
|
||||
|
||||
//----------------------------------- components ----------------------------------
|
||||
|
||||
static void _traverse ( SrGraphBase* graph,
|
||||
SrArray<SrGraphNode*>& stack,
|
||||
SrArray<SrGraphNode*>& nodes )
|
||||
{
|
||||
int i;
|
||||
SrGraphNode *n, *ln;
|
||||
while ( stack.size()>0 )
|
||||
{ n = stack.pop();
|
||||
graph->mark ( n );
|
||||
nodes.push() = n;
|
||||
for ( i=0; i<n->num_links(); i++ )
|
||||
{ ln = n->link(i)->node();
|
||||
if ( !graph->marked(ln) ) stack.push()=ln;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrGraphBase::get_connected_nodes ( SrGraphNode* source, SrArray<SrGraphNode*>& nodes )
|
||||
{
|
||||
nodes.size ( num_nodes() );
|
||||
nodes.size ( 0 );
|
||||
|
||||
SrArray<SrGraphNode*>& stack = _buffer;
|
||||
stack.size ( 0 );
|
||||
|
||||
begin_marking();
|
||||
stack.push() = source;
|
||||
_traverse ( this, stack, nodes );
|
||||
end_marking();
|
||||
}
|
||||
|
||||
void SrGraphBase::get_disconnected_components ( SrArray<int>& components, SrArray<SrGraphNode*>& nodes )
|
||||
{
|
||||
nodes.size ( num_nodes() );
|
||||
nodes.size ( 0 );
|
||||
|
||||
components.size ( 0 );
|
||||
|
||||
SrArray<SrGraphNode*>& stack = _buffer;
|
||||
stack.size ( 0 );
|
||||
|
||||
SrGraphNode* n;
|
||||
SrListIterator<SrGraphNode> it(_nodes);
|
||||
|
||||
begin_marking();
|
||||
|
||||
for ( it.first(); it.inrange(); it.next() )
|
||||
{ n = it.get();
|
||||
if ( !marked(n) )
|
||||
{ components.push() = nodes.size();
|
||||
stack.push() = n;
|
||||
_traverse ( this, stack, nodes );
|
||||
components.push() = nodes.size()-1;
|
||||
}
|
||||
}
|
||||
|
||||
end_marking();
|
||||
}
|
||||
|
||||
//----------------------------------- shortest path ----------------------------------
|
||||
|
||||
float SrGraphBase::get_shortest_path ( SrGraphNode* n1,
|
||||
SrGraphNode* n2,
|
||||
SrArray<SrGraphNode*>& path,
|
||||
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata ),
|
||||
void* udata )
|
||||
{
|
||||
path.size(0);
|
||||
if ( n1==n2 ) { path.push()=n1; return 0; }
|
||||
|
||||
begin_marking ();
|
||||
|
||||
if ( !_pt ) _pt = new SrGraphPathTree;
|
||||
_pt->init ( this, n2 );
|
||||
|
||||
if ( distfunc )
|
||||
{ _pt->distfunc = distfunc;
|
||||
_pt->udata = udata;
|
||||
}
|
||||
|
||||
int i;
|
||||
bool end = false;
|
||||
|
||||
while ( !end )
|
||||
{ if ( !_pt->has_leaf() )
|
||||
{ end_marking();
|
||||
if ( !distfunc ) return 0;
|
||||
i = _pt->iclosest;
|
||||
float cost = _pt->nodes[i].cost;
|
||||
while ( i>=0 )
|
||||
{ path.push () = _pt->nodes[i].node;
|
||||
i = _pt->nodes[i].parent;
|
||||
}
|
||||
return cost;
|
||||
}
|
||||
|
||||
end = _pt->expand_lowest_cost_leaf ( n1 );
|
||||
}
|
||||
|
||||
end_marking ();
|
||||
|
||||
i = _pt->nodes.size()-1; // last element is n1
|
||||
float cost = _pt->nodes[i].cost;
|
||||
while ( i>=0 )
|
||||
{ path.push () = _pt->nodes[i].node;
|
||||
i = _pt->nodes[i].parent;
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
bool SrGraphBase::local_search
|
||||
( SrGraphNode* startn,
|
||||
SrGraphNode* endn,
|
||||
int maxdepth,
|
||||
float maxdist,
|
||||
int& depth,
|
||||
float& dist )
|
||||
{
|
||||
depth=0;
|
||||
dist=0;
|
||||
|
||||
if ( startn==endn ) return true;
|
||||
|
||||
begin_marking ();
|
||||
|
||||
if ( !_pt ) _pt = new SrGraphPathTree;
|
||||
_pt->init ( this, startn );
|
||||
|
||||
int i;
|
||||
bool not_found = false;
|
||||
bool end = false;
|
||||
|
||||
while ( !end )
|
||||
{ if ( !_pt->has_leaf() ) { not_found=true; break; } // not found!
|
||||
|
||||
dist = _pt->nodes[_pt->leafs.top().l].cost;
|
||||
depth = _pt->leafs.top().d;
|
||||
|
||||
if ( maxdepth>0 && depth>maxdepth ) { i=-1; break; } // max depth reached
|
||||
if ( maxdist>0 && dist>maxdist ) { i=-1; break; } // max dist reached
|
||||
|
||||
end = _pt->expand_lowest_cost_leaf ( endn );
|
||||
}
|
||||
|
||||
end_marking ();
|
||||
|
||||
if ( not_found ) return false; // not found!
|
||||
return true;
|
||||
}
|
||||
|
||||
void SrGraphBase::bidirectional_block_test ( bool b )
|
||||
{
|
||||
if ( !_pt ) _pt = new SrGraphPathTree;
|
||||
_pt->bidirectional_block = b;
|
||||
}
|
||||
|
||||
//------------------------------------- I/O --------------------------------
|
||||
|
||||
void SrGraphBase::output ( SrOutput& o )
|
||||
{
|
||||
SrListIterator<SrGraphNode> it(_nodes);
|
||||
SrClassManagerBase* nman = node_class_manager();
|
||||
|
||||
// set indices
|
||||
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::operator<<(): begin_indexing() is locked!");
|
||||
sruint i=0;
|
||||
begin_indexing();
|
||||
for ( it.first(); it.inrange(); it.next() ) index(it.get(),i++);
|
||||
|
||||
// print
|
||||
o<<'[';
|
||||
for ( it.first(); it.inrange(); it.next() )
|
||||
{ o << it->index() << srspc; // output node index
|
||||
o << it->_blocked << srspc; // output node blocked status
|
||||
nman->output ( o, it.get() ); // output user data
|
||||
if ( it.get()->num_links()>0 ) // output node links (blocked, ids, cost, and udata)
|
||||
{ o<<srspc;
|
||||
it->SrGraphNode::output ( o );
|
||||
}
|
||||
if ( !it.inlast() ) o << srnl;
|
||||
}
|
||||
o<<']';
|
||||
|
||||
if ( !_leave_indices_after_save ) end_indexing();
|
||||
_leave_indices_after_save = 0;
|
||||
}
|
||||
|
||||
static void set_blocked ( int& blocked, const char* last_token )
|
||||
{
|
||||
if ( last_token[0]=='b' ) // back-compatibility
|
||||
blocked = 1;
|
||||
else if ( last_token[0]=='f' ) // back-compatibility
|
||||
blocked = 0;
|
||||
else // should be an integer
|
||||
blocked = atoi(last_token);
|
||||
}
|
||||
|
||||
void SrGraphBase::input ( SrInput& inp )
|
||||
{
|
||||
SrArray<SrGraphNode*>& nodes = _buffer;
|
||||
nodes.size(128);
|
||||
nodes.size(0);
|
||||
|
||||
SrClassManagerBase* nman = node_class_manager();
|
||||
SrClassManagerBase* lman = link_class_manager();
|
||||
|
||||
init ();
|
||||
|
||||
inp.getd(); // [
|
||||
inp.get_token(); // get node counter (which is not needed)
|
||||
|
||||
while ( inp.last_token()[0]!=']' )
|
||||
{
|
||||
nodes.push() = _nodes.insert_next(); // allocate one node
|
||||
nodes.top()->_graph = this;
|
||||
|
||||
inp.get_token(); // get node blocked status
|
||||
set_blocked ( nodes.top()->_blocked, inp.last_token() );
|
||||
|
||||
nman->input ( inp, nodes.top() ); // read node user data
|
||||
|
||||
inp.get_token(); // new node counter, or '(', or ']'
|
||||
|
||||
if ( inp.last_token()[0]=='(')
|
||||
while ( true )
|
||||
{ inp.get_token();
|
||||
if ( inp.last_token()[0]==')' ) { inp.get_token(); break; }
|
||||
SrArray<SrGraphLink*>& la = nodes.top()->_links;
|
||||
la.push() = (SrGraphLink*) lman->alloc();
|
||||
set_blocked ( la.top()->_blocked, inp.last_token() ); // get link blocked status
|
||||
|
||||
inp.get_token(); // get id
|
||||
la.top()->_index = atoi(inp.last_token()); // store id
|
||||
inp.getn(); // get cost
|
||||
la.top()->_cost = (float) atof(inp.last_token()); // store cost
|
||||
lman->input ( inp, la.top() );
|
||||
}
|
||||
}
|
||||
|
||||
// now convert indices to pointers:
|
||||
int i, j;
|
||||
for ( i=0; i<nodes.size(); i++ )
|
||||
{ SrArray<SrGraphLink*>& la = nodes[i]->_links;
|
||||
for ( j=0; j<la.size(); j++ )
|
||||
{ la[j]->_node = nodes[ int(la[j]->_index) ];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//---------------------------- private methods --------------------------------
|
||||
|
||||
// set all indices (nodes and links) to 0 and curmark to 1
|
||||
void SrGraphBase::_normalize_mark()
|
||||
{
|
||||
int i;
|
||||
SrGraphNode* n;
|
||||
|
||||
if ( _nodes.empty() ) return;
|
||||
|
||||
_nodes.gofirst();
|
||||
do { n = _nodes.cur();
|
||||
n->_index = 0;
|
||||
for ( i=0; i<n->num_links(); i++ ) n->link(i)->_index=0;
|
||||
} while ( _nodes.notlast() );
|
||||
_curmark = 1;
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
320
source/dcdt/se/sr_graph.h
Normal file
320
source/dcdt/se/sr_graph.h
Normal file
@ -0,0 +1,320 @@
|
||||
|
||||
# ifndef SR_GRAPH_H
|
||||
# define SR_GRAPH_H
|
||||
|
||||
/** \file sr_graph.h
|
||||
* Graph maintenance classes
|
||||
*/
|
||||
|
||||
# include "sr_array.h"
|
||||
# include "sr_list.h"
|
||||
|
||||
class SrGraphNode;
|
||||
class SrGraphBase;
|
||||
|
||||
/*! SrGraphLink contains the minimal needed information for
|
||||
a link. To attach user-related information, SrGraphLink
|
||||
must be derived and as well a corresponding
|
||||
SrClassManagerBased, which will manage only the user data. */
|
||||
class SrGraphLink
|
||||
{ private :
|
||||
SrGraphNode* _node; // node pointing to
|
||||
sruint _index; // index for traversing
|
||||
float _cost; // cost for minimum paths
|
||||
int _blocked; // used as boolean or as a ref counter
|
||||
friend class SrGraphNode;
|
||||
friend class SrGraphBase;
|
||||
protected :
|
||||
SrGraphLink () { _index=0; _cost=0; _node=0; _blocked=0; }
|
||||
~SrGraphLink () {}
|
||||
public :
|
||||
void cost ( float c ) { _cost=c; }
|
||||
float cost () const { return _cost; }
|
||||
SrGraphNode* node () const { return _node; }
|
||||
sruint index () const { return _index; }
|
||||
int blocked () const { return _blocked; }
|
||||
void blocked ( bool b ) { _blocked = b? 1:0; }
|
||||
void blocked ( int b ) { _blocked = b; }
|
||||
};
|
||||
|
||||
/*! SrGraphNode contains the minimal needed information for
|
||||
a node. To attach user-related information, SrGraphLink
|
||||
must be derived and as well a corresponding
|
||||
SrClassManagerBased, which will manage only the user data. */
|
||||
class SrGraphNode : public SrListNode
|
||||
{ private :
|
||||
SrArray<SrGraphLink*> _links;
|
||||
sruint _index;
|
||||
int _blocked; // used as boolean or as a ref counter
|
||||
SrGraphBase* _graph;
|
||||
friend class SrGraphBase;
|
||||
protected :
|
||||
SrGraphNode () { _index=0; _graph=0; _blocked=0; }
|
||||
~SrGraphNode ();
|
||||
|
||||
public :
|
||||
|
||||
/*! Returns the next node in the global list of nodes */
|
||||
SrGraphNode* next() const { return (SrGraphNode*) SrListNode::next(); }
|
||||
|
||||
/*! Returns the prior node in the global list of nodes */
|
||||
SrGraphNode* prior() const { return (SrGraphNode*) SrListNode::prior(); }
|
||||
|
||||
/*! Returns the index of this node */
|
||||
sruint index () { return _index; }
|
||||
|
||||
int blocked () const { return _blocked; }
|
||||
void blocked ( bool b ) { _blocked = b? 1:0; }
|
||||
void blocked ( int b ) { _blocked = b; }
|
||||
|
||||
/*! Compress internal links array */
|
||||
void compress () { _links.compress(); }
|
||||
|
||||
/*! Returns the new link created */
|
||||
SrGraphLink* linkto ( SrGraphNode* n, float cost=0 );
|
||||
|
||||
/*! Remove link of index ni, with a fast remove: the order in the
|
||||
links array is not mantained */
|
||||
void unlink ( int ni );
|
||||
|
||||
/*! Remove the link pointing to n. Should be called ONLY when it is
|
||||
guaranteed that the link to n really exists! Uses method unlink(ni) */
|
||||
void unlink ( SrGraphNode* n ) { unlink ( search_link(n) ); }
|
||||
|
||||
/*! Returns the index in the links array, or -1 if not linked to n */
|
||||
int search_link ( SrGraphNode* n ) const;
|
||||
|
||||
/*! Returns the link pointing to n. Should be called ONLY when it is
|
||||
guaranteed that the link to n really exists! */
|
||||
SrGraphLink* link ( SrGraphNode*n ) { return _links[search_link(n)]; }
|
||||
|
||||
/*! Returns the link index i, which must be a valid index */
|
||||
SrGraphLink* link ( int i ) { return _links[i]; }
|
||||
|
||||
/*! Returns the last link */
|
||||
SrGraphLink* last_link () { return _links.top(); }
|
||||
|
||||
/*! Returns the number of links in this node */
|
||||
int num_links () const { return _links.size(); }
|
||||
|
||||
/*! Returns the links array */
|
||||
const SrArray<SrGraphLink*>& links() const { return _links; }
|
||||
|
||||
private :
|
||||
void output ( SrOutput& o ) const;
|
||||
};
|
||||
|
||||
/*! The following define is to be used inside a user derived class
|
||||
of SrGraphLink, in order to easily redefine public SrGraphLink
|
||||
methods with correct type casts. */
|
||||
# define SR_GRAPH_LINK_CASTED_METHODS(N,L) \
|
||||
N* node() const { return (N*)SrGraphLink::node(); }
|
||||
|
||||
/*! The following define is to be used inside a user derived class
|
||||
of SrGraphNode, in order to easily redefine public SrGraphNode
|
||||
methods with correct type casts. */
|
||||
# define SR_GRAPH_NODE_CASTED_METHODS(N,L) \
|
||||
N* next() const { return (N*)SrListNode::next(); } \
|
||||
N* prior() const { return (N*)SrListNode::prior(); } \
|
||||
L* linkto ( N* n, float c ) { return (L*)SrGraphNode::linkto(n,c); } \
|
||||
L* link ( N* n ) { return (L*)SrGraphNode::link(n); } \
|
||||
L* link ( int i ) { return (L*)SrGraphNode::link(i); } \
|
||||
const SrArray<L*>& links() const { return (const SrArray<L*>&) SrGraphNode::links(); }
|
||||
|
||||
class SrGraphPathTree;
|
||||
|
||||
/*! SrGraphBase maintains a directed graph with nodes and links. Links around
|
||||
a node do not have any order meaning.
|
||||
Note also that the user should avoid redundant links, as no tests are done
|
||||
(for speed purposes) in several methods.
|
||||
SrGraphBase is the base class, the user should use SrGraph template instead. */
|
||||
class SrGraphBase
|
||||
{ private :
|
||||
SrList<SrGraphNode> _nodes;
|
||||
SrArray<SrGraphNode*> _buffer;
|
||||
sruint _curmark;
|
||||
char _mark_status;
|
||||
SrGraphPathTree* _pt;
|
||||
SrClassManagerBase* _lman; // link manager for a class deriving SrGraphLink
|
||||
char _leave_indices_after_save;
|
||||
|
||||
public :
|
||||
/*! Constructor requires managers for nodes and links */
|
||||
SrGraphBase ( SrClassManagerBase* nm, SrClassManagerBase* lm );
|
||||
|
||||
/*! Destructor deletes all data in the graph */
|
||||
~SrGraphBase ();
|
||||
|
||||
/*! Returns a pointer to the node manager */
|
||||
SrClassManagerBase* node_class_manager() const { return _nodes.class_manager(); }
|
||||
|
||||
/*! Returns a pointer to the link manager */
|
||||
SrClassManagerBase* link_class_manager() const { return _lman; }
|
||||
|
||||
/*! Make the graph empty */
|
||||
void init ();
|
||||
|
||||
/*! Compress all link arrays in the nodes */
|
||||
void compress ();
|
||||
|
||||
/*! Returns the number of nodes in the graph */
|
||||
int num_nodes () const { return _nodes.elements(); }
|
||||
|
||||
/*! Counts and returns the number of (directional) links in the graph */
|
||||
int num_links () const;
|
||||
|
||||
/*! Methods for marking nodes and links */
|
||||
void begin_marking ();
|
||||
void end_marking ();
|
||||
bool marked ( SrGraphNode* n );
|
||||
void mark ( SrGraphNode* n );
|
||||
void unmark ( SrGraphNode* n );
|
||||
bool marked ( SrGraphLink* l );
|
||||
void mark ( SrGraphLink* l );
|
||||
void unmark ( SrGraphLink* l );
|
||||
|
||||
/*! Methods for indexing nodes and links */
|
||||
void begin_indexing ();
|
||||
void end_indexing ();
|
||||
sruint index ( SrGraphNode* n );
|
||||
void index ( SrGraphNode* n, sruint i );
|
||||
sruint index ( SrGraphLink* l );
|
||||
void index ( SrGraphLink* l, sruint i );
|
||||
|
||||
/*! Returns the list of nodes, that should be used with
|
||||
care to not invalidate some SrGraph operations. */
|
||||
SrList<SrGraphNode>& nodes () { return _nodes; }
|
||||
|
||||
/*! Inserts node n in the list of nodes, as a new
|
||||
unconnected component. n is returned. */
|
||||
SrGraphNode* insert ( SrGraphNode* n );
|
||||
|
||||
/*! Extract (without deleting) the node from the graph. Nothing
|
||||
is done concerning possible links between the graph and
|
||||
the node being extracted */
|
||||
SrGraphNode* extract ( SrGraphNode* n );
|
||||
|
||||
/*! Removes and deletes the node from the graph. Its the user
|
||||
responsibility to ensure that the graph has no links with
|
||||
the node being deleted */
|
||||
void remove_node ( SrGraphNode* n );
|
||||
|
||||
/*! Searches and removes the edge(s) linking n1 with n2 if any.
|
||||
Links are removed with a "fast remove process" so that indices
|
||||
in the links array of the involved nodes may be modified.
|
||||
Return the number of (directed) links removed. */
|
||||
int remove_link ( SrGraphNode* n1, SrGraphNode* n2 );
|
||||
|
||||
/*! Links n1 to n2 and n2 to n1, with cost c in both directions */
|
||||
void link ( SrGraphNode* n1, SrGraphNode* n2, float c=0 );
|
||||
|
||||
/*! Returns the first node of the list of nodes */
|
||||
SrGraphNode* first_node () const { return _nodes.first(); }
|
||||
|
||||
/*! Get all directed edges of the graph. Note that if n1 is linked
|
||||
to n2 and n2 is linked to n1, both edges (n1,n2) and (n2,n1) will
|
||||
appear in the edges array */
|
||||
void get_directed_edges ( SrArray<SrGraphNode*>& edges );
|
||||
|
||||
/*! Get all edges of the graph without duplications from the directional
|
||||
point of view. */
|
||||
void get_undirected_edges ( SrArray<SrGraphNode*>& edges );
|
||||
|
||||
/*! Get all nodes which are in the same connected component of source */
|
||||
void get_connected_nodes ( SrGraphNode* source, SrArray<SrGraphNode*>& nodes );
|
||||
|
||||
/*! Organize nodes by connected components. The indices in array components say each
|
||||
start and end position in array nodes for each component */
|
||||
void get_disconnected_components ( SrArray<int>& components, SrArray<SrGraphNode*>& nodes );
|
||||
|
||||
/*! The returned path contains pointers to existing nodes in the graph.
|
||||
In the case the two nodes are in two different disconnected components
|
||||
an empty path is returned. If n1==n2 a path with the single node n1
|
||||
is returned. In all cases, returns the distance (cost) of path.
|
||||
In case no path is found, the optional parameters distfunc and udata
|
||||
can be used to return the path to the closest processed node to the goal. */
|
||||
float get_shortest_path ( SrGraphNode* n1, SrGraphNode* n2, SrArray<SrGraphNode*>& path,
|
||||
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata )=0,
|
||||
void* udata=0 );
|
||||
|
||||
/*! Performs a A* search from startn, until finding endn. The search is
|
||||
stopped if either maxnodes or maxdist is reached. If these parameters
|
||||
are <0 they are not taken into account. True is returned if endn could
|
||||
be reached, and parameters depth and dist will contain the final
|
||||
higher depth and distance (along the shortest path) reached. */
|
||||
bool local_search ( SrGraphNode* startn, SrGraphNode* endn,
|
||||
int maxnodes, float maxdepth, int& depth, float& dist );
|
||||
|
||||
/*! When set to true, graph search will consider a link blocked if any of its
|
||||
directions is blocked, i.e., blocking only one direction of a link will
|
||||
block both the directions. Afftected methods are get_shortest_path() and
|
||||
local_search(). Default is false. */
|
||||
void bidirectional_block_test ( bool b );
|
||||
|
||||
bool are_near_in_graph ( SrGraphNode* startn, SrGraphNode* endn, int maxnodes );
|
||||
|
||||
/*! If this is set to true, it will be the user responsibility to call
|
||||
end_indexing() after the next graph save. It is used to retrieve the indices
|
||||
used during saving to reference additional data to be saved in derived classes. */
|
||||
void leave_indices_after_save ( bool b ) { _leave_indices_after_save = b? 1:0; }
|
||||
|
||||
/*! Returns the internal buffer, which might be usefull for some specific needs;
|
||||
see for instance the description of the input() method. */
|
||||
SrArray<SrGraphNode*>& buffer () { return _buffer; }
|
||||
|
||||
/*! Outputs the graph in the format: [ (l1..lk)e1 (..)e2 (..)en ].
|
||||
Nodes are indexed (starting from 0), and after the output
|
||||
all indices of the nodes are set to 0. */
|
||||
void output ( SrOutput& o );
|
||||
|
||||
/*! Initializes the current graph and load another one from the given input.
|
||||
Method buffer() can be used to retrieve an array with all nodes loaded,
|
||||
indexed by the indices used in the loaded file */
|
||||
void input ( SrInput& i );
|
||||
|
||||
friend SrOutput& operator<< ( SrOutput& o, SrGraphBase& g ) { g.output(o); return o; }
|
||||
|
||||
friend SrInput& operator>> ( SrInput& i, SrGraphBase& g ) { g.input(i); return i; }
|
||||
|
||||
private :
|
||||
void _normalize_mark();
|
||||
|
||||
};
|
||||
|
||||
/*! SrGraph is a template that includes type casts for the user
|
||||
derived types (of SrGraphNode and SrGraphLink) to correctly
|
||||
call SrGraphBase methods. */
|
||||
template <class N, class L>
|
||||
class SrGraph : public SrGraphBase
|
||||
{ public :
|
||||
SrGraph () : SrGraphBase ( new SrClassManager<N>, new SrClassManager<L> ) {}
|
||||
|
||||
SrGraph ( SrClassManagerBase* nm, SrClassManagerBase* lm ) : SrGraphBase ( nm, lm ) {}
|
||||
|
||||
const SrList<N>& nodes () { return (SrList<N>&) SrGraphBase::nodes(); }
|
||||
|
||||
N* insert ( N* n ) { return (N*) SrGraphBase::insert((SrGraphNode*)n); }
|
||||
N* extract ( N* n ) { return (N*) SrGraphBase::extract((SrGraphNode*)n); }
|
||||
N* first_node () const { return (N*) SrGraphBase::first_node(); }
|
||||
|
||||
void get_undirected_edges ( SrArray<N*>& edges )
|
||||
{ SrGraphBase::get_undirected_edges( (SrArray<SrGraphNode*>&)edges ); }
|
||||
|
||||
void get_disconnected_components ( SrArray<int>& components, SrArray<N*>& nodes )
|
||||
{ SrGraphBase::get_disconnected_components( components, (SrArray<SrGraphNode*>&)nodes ); }
|
||||
|
||||
float get_shortest_path ( N* n1, N* n2, SrArray<N*>& path,
|
||||
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata )=0,
|
||||
void* udata=0 )
|
||||
{ return SrGraphBase::get_shortest_path((SrGraphNode*)n1,(SrGraphNode*)n2,(SrArray<SrGraphNode*>&)path,distfunc,udata); }
|
||||
|
||||
SrArray<N*>& buffer () { return (SrArray<N*>&) SrGraphBase::buffer(); }
|
||||
|
||||
friend SrOutput& operator<< ( SrOutput& o, SrGraph& g ) { return o<<(SrGraphBase&)g; }
|
||||
friend SrInput& operator>> ( SrInput& i, SrGraph& g ) { return i>>(SrGraphBase&)g; }
|
||||
};
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_GRAPH_H
|
||||
|
||||
249
source/dcdt/se/sr_grid.cpp
Normal file
249
source/dcdt/se/sr_grid.cpp
Normal file
@ -0,0 +1,249 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_grid.h"
|
||||
|
||||
//============================ SrGridBase =================================
|
||||
|
||||
SrGridBase::SrGridBase ( int dim, int ns )
|
||||
{
|
||||
_cells = 0;
|
||||
if ( dim*ns>0 ) init ( dim, ns );
|
||||
}
|
||||
|
||||
void SrGridBase::init ( int dim, int ns )
|
||||
{
|
||||
int i;
|
||||
_axis.size ( dim );
|
||||
|
||||
for ( i=0; i<dim; i++ )
|
||||
{ _axis[i].min = 0.0f;
|
||||
_axis[i].max = 1.0f;
|
||||
_axis[i].segs = ns;
|
||||
}
|
||||
|
||||
init ( _axis );
|
||||
}
|
||||
|
||||
void SrGridBase::init ( const SrArray<SrGridAxis>& axis_desc )
|
||||
{
|
||||
if ( &axis_desc != &_axis ) _axis=axis_desc;
|
||||
|
||||
if ( _axis.size()==0 ) { _cells=0; _size.size(0); _seglen.size(0); return; }
|
||||
|
||||
int i, j;
|
||||
|
||||
_cells = _axis[0].segs;
|
||||
int dim = _axis.size();
|
||||
|
||||
for ( i=1; i<dim; i++ )
|
||||
{ _cells *= _axis[i].segs;
|
||||
}
|
||||
|
||||
_size.size ( dim );
|
||||
for ( i=0; i<dim; i++ )
|
||||
{ _size[i] = 1;
|
||||
for ( j=0; j<i; j++ ) _size[i]*=_axis[j].segs;
|
||||
}
|
||||
|
||||
_seglen.size ( dim );
|
||||
for ( i=0; i<dim; i++ )
|
||||
_seglen[i] = (_axis[i].max-_axis[i].min)/float(_axis[i].segs);
|
||||
}
|
||||
|
||||
int SrGridBase::cell_index ( int i, int j ) const
|
||||
{
|
||||
if ( dimensions()!=2 ) return 0;
|
||||
|
||||
return _size[1]*j + i;
|
||||
}
|
||||
|
||||
int SrGridBase::cell_index ( int i, int j, int k ) const
|
||||
{
|
||||
if ( dimensions()!=3 ) return 0;
|
||||
|
||||
return _size[2]*k + _size[1]*j + i;
|
||||
}
|
||||
|
||||
int SrGridBase::cell_index ( const SrArray<int>& coords ) const
|
||||
{
|
||||
int dim = _axis.size();
|
||||
|
||||
if ( dim!=coords.size() || dim<=0 ) return 0;
|
||||
|
||||
int i, cell = coords[0];
|
||||
for ( i=1; i<dim; i++ ) cell += coords[i]*_size[i];
|
||||
|
||||
return cell;
|
||||
}
|
||||
|
||||
void SrGridBase::cell_coords ( int index, int& i, int& j ) const
|
||||
{
|
||||
if ( dimensions()!=2 ) { i=j=0; return; }
|
||||
|
||||
j = index/_size[1]; index=index%_size[1];
|
||||
i = index;
|
||||
}
|
||||
|
||||
void SrGridBase::cell_coords ( int index, int& i, int& j, int& k ) const
|
||||
{
|
||||
if ( dimensions()!=3 ) { i=j=k=0; return; }
|
||||
|
||||
k = index/_size[2]; index=index%_size[2];
|
||||
j = index/_size[1]; index=index%_size[1];
|
||||
i = index;
|
||||
}
|
||||
|
||||
void SrGridBase::cell_coords ( int index, SrArray<int>& coords ) const
|
||||
{
|
||||
int d = dimensions();
|
||||
|
||||
if ( coords.size()!=d ) coords.size(d);
|
||||
|
||||
d--;
|
||||
while ( d>0 )
|
||||
{ coords[d] = index/_size[d];
|
||||
index = index%_size[d];
|
||||
d--;
|
||||
}
|
||||
coords[0] = index;
|
||||
}
|
||||
|
||||
void SrGridBase::cell_boundary ( int i, int j, SrPnt2& a, SrPnt2& b ) const
|
||||
{
|
||||
if ( dimensions()!=2 ) return;
|
||||
|
||||
a.x = _axis[0].min + float(i)*_seglen[0];
|
||||
a.y = _axis[1].min + float(j)*_seglen[1];
|
||||
|
||||
b.x = a.x + _seglen[0];
|
||||
b.y = a.y + _seglen[1];
|
||||
}
|
||||
|
||||
void SrGridBase::cell_boundary ( int i, int j, int k, SrPnt& a, SrPnt& b ) const
|
||||
{
|
||||
if ( dimensions()!=3 ) return;
|
||||
|
||||
a.x = _axis[0].min + float(i)*_seglen[0];
|
||||
a.y = _axis[1].min + float(j)*_seglen[1];
|
||||
a.z = _axis[2].min + float(k)*_seglen[2];
|
||||
|
||||
b.x = a.x + _seglen[0];
|
||||
b.y = a.y + _seglen[1];
|
||||
b.z = a.z + _seglen[2];
|
||||
}
|
||||
|
||||
void SrGridBase::get_intersection ( SrPnt2 a, SrPnt2 b, SrArray<int>& cells ) const
|
||||
{
|
||||
if ( dimensions()!=2 ) return;
|
||||
|
||||
if ( a.x>b.x || a.y>b.y ) return;
|
||||
|
||||
if ( a.x>=_axis[0].max || a.y>=_axis[1].max ) return;
|
||||
if ( b.x<=_axis[0].min || b.y<=_axis[1].min ) return;
|
||||
|
||||
float f = _seglen[0]/2;
|
||||
a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-f );
|
||||
b.x = SR_BOUND ( b.x, _axis[0].min, _axis[0].max-f );
|
||||
f = _seglen[1]/2;
|
||||
a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-f );
|
||||
b.y = SR_BOUND ( b.y, _axis[1].min, _axis[1].max-f );
|
||||
|
||||
int i1 = (int) ((a.x-_axis[0].min) / _seglen[0]);
|
||||
int j1 = (int) ((a.y-_axis[1].min) / _seglen[1]);
|
||||
|
||||
f = (b.x-_axis[0].min) / _seglen[0];
|
||||
int i2 = int(f); if ( float(i2)==f ) i2--;
|
||||
|
||||
f = (b.y-_axis[1].min) / _seglen[1];
|
||||
int j2 = int(f); if ( float(j2)==f ) j2--;
|
||||
|
||||
int i, j, index;
|
||||
|
||||
for ( j=j1; j<=j2; j++ )
|
||||
{ index = _size[1]*j + i1; // == cell_index(i1,j);
|
||||
for ( i=i1; i<=i2; i++ )
|
||||
{ cells.push() = index;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SrGridBase::get_intersection ( SrPnt a, SrPnt b, SrArray<int>& cells ) const
|
||||
{
|
||||
if ( dimensions()!=3 ) return;
|
||||
|
||||
if ( a.x>b.x || a.y>b.y || a.z>b.z ) return;
|
||||
|
||||
if ( a.x>=_axis[0].max || a.y>=_axis[1].max || a.z>=_axis[2].max ) return;
|
||||
if ( b.x<=_axis[0].min || b.y<=_axis[1].min || b.z<=_axis[2].min ) return;
|
||||
|
||||
float f = _seglen[0]/2;
|
||||
a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-f );
|
||||
b.x = SR_BOUND ( b.x, _axis[0].min, _axis[0].max-f );
|
||||
f = _seglen[1]/2;
|
||||
a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-f );
|
||||
b.y = SR_BOUND ( b.y, _axis[1].min, _axis[1].max-f );
|
||||
f = _seglen[2]/2;
|
||||
a.z = SR_BOUND ( a.z, _axis[2].min, _axis[2].max-f );
|
||||
b.z = SR_BOUND ( b.z, _axis[2].min, _axis[2].max-f );
|
||||
|
||||
int i1 = (int) ((a.x-_axis[0].min) / _seglen[0]);
|
||||
int j1 = (int) ((a.y-_axis[1].min) / _seglen[1]);
|
||||
int k1 = (int) ((a.z-_axis[2].min) / _seglen[2]);
|
||||
|
||||
f = (b.x-_axis[0].min) / _seglen[0];
|
||||
int i2 = int(f); if ( float(i2)==f ) i2--;
|
||||
|
||||
f = (b.y-_axis[1].min) / _seglen[1];
|
||||
int j2 = int(f); if ( float(j2)==f ) j2--;
|
||||
|
||||
f = (b.z-_axis[2].min) / _seglen[2];
|
||||
int k2 = int(f); if ( float(k2)==f ) k2--;
|
||||
|
||||
int i, j, k, index;
|
||||
|
||||
for ( k=k1; k<=k2; k++ )
|
||||
for ( j=j1; j<=j2; j++ )
|
||||
{ index = _size[2]*k + _size[1]*j + i1; // == cell_index(i1,j,k);
|
||||
for ( i=i1; i<=i2; i++ )
|
||||
{ cells.push() = index;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int SrGridBase::get_point_location ( SrPnt2 a ) const
|
||||
{
|
||||
if ( dimensions()!=2 ) return -1;
|
||||
|
||||
if ( a.x>=_axis[0].max || a.y>=_axis[1].max ) return -1;
|
||||
if ( a.x<=_axis[0].min || a.y<=_axis[1].min ) return -1;
|
||||
|
||||
a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-(_seglen[0]/2) );
|
||||
a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-(_seglen[1]/2) );
|
||||
|
||||
int i = (int) ((a.x-_axis[0].min) / _seglen[0]);
|
||||
int j = (int) ((a.y-_axis[1].min) / _seglen[1]);
|
||||
|
||||
return _size[1]*j + i;
|
||||
}
|
||||
|
||||
int SrGridBase::get_point_location ( SrPnt a ) const
|
||||
{
|
||||
if ( dimensions()!=3 ) return -1;
|
||||
|
||||
if ( a.x>=_axis[0].max || a.y>=_axis[1].max || a.z>=_axis[2].max ) return -1;
|
||||
if ( a.x<=_axis[0].min || a.y<=_axis[1].min || a.z<=_axis[2].min ) return -1;
|
||||
|
||||
a.x = SR_BOUND ( a.x, _axis[0].min, _axis[0].max-(_seglen[0]/2) );
|
||||
a.y = SR_BOUND ( a.y, _axis[1].min, _axis[1].max-(_seglen[1]/2) );
|
||||
a.z = SR_BOUND ( a.z, _axis[2].min, _axis[2].max-(_seglen[2]/2) );
|
||||
|
||||
int i = (int) ((a.x-_axis[0].min) / _seglen[0]);
|
||||
int j = (int) ((a.y-_axis[1].min) / _seglen[1]);
|
||||
int k = (int) ((a.z-_axis[2].min) / _seglen[2]);
|
||||
|
||||
return _size[2]*k + _size[1]*j + i;
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
167
source/dcdt/se/sr_grid.h
Normal file
167
source/dcdt/se/sr_grid.h
Normal file
@ -0,0 +1,167 @@
|
||||
|
||||
# ifndef SR_GRID_H
|
||||
# define SR_GRID_H
|
||||
|
||||
/** \file sr_grid.h
|
||||
* Manages data in a n-D regular grid */
|
||||
|
||||
# include "sr_vec.h"
|
||||
# include "sr_vec2.h"
|
||||
# include "sr_array.h"
|
||||
# include "sr_output.h"
|
||||
# include "sr_class_manager.h"
|
||||
|
||||
/*! Describes how to subdivide one dimensional axis.
|
||||
This class is required to set up the decomposition in SrGrid */
|
||||
class SrGridAxis
|
||||
{ public :
|
||||
int segs; // number of segments to divide this axis
|
||||
float min; // minimum coordinate
|
||||
float max; // maximum coordinate
|
||||
|
||||
public :
|
||||
SrGridAxis ( int se, float mi, float ma )
|
||||
{ segs=se; min=mi; max=ma; }
|
||||
|
||||
void set ( int se, float mi, float ma )
|
||||
{ segs=se; min=mi; max=ma; }
|
||||
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrGridAxis& a )
|
||||
{ return o << a.segs << srspc << a.min << srspc << a.max; }
|
||||
|
||||
friend SrInput& operator>> ( SrInput& i, SrGridAxis& a )
|
||||
{ return i >> a.segs >> a.min >> a.max; }
|
||||
};
|
||||
|
||||
/*! \class SrGridBase sr_grid.h
|
||||
\brief n-D regular grid base class
|
||||
|
||||
SrGridBase has methods to translate n dimensional grid coordinates
|
||||
into a one dimension bucket array coordinate. Use the derived SrGrid
|
||||
template class to associate user data to the grid. */
|
||||
class SrGridBase
|
||||
{ private :
|
||||
SrArray<SrGridAxis> _axis;
|
||||
SrArray<int> _size; // subspaces sizes for fast bucket determination
|
||||
SrArray<float> _seglen; // axis segments lengths for fast cell determination
|
||||
int _cells;
|
||||
|
||||
public :
|
||||
|
||||
/*! Constructs a grid of given dimension (dim) and number of
|
||||
segments in each axis (ns). The number of cells will then be
|
||||
ns^dim. Axis are normalized in [0,1] */
|
||||
SrGridBase ( int dim=0, int ns=0 );
|
||||
|
||||
/*! Init the grid with given dimension and number of
|
||||
segments in each axis. The number of cells will then be
|
||||
ns^dim. Axis are normalized in [0,1] */
|
||||
void init ( int dim, int ns );
|
||||
|
||||
/*! Destroy the existing grid and initializes one according to
|
||||
the descriptions sent in the desc array, which size will
|
||||
define the dimension of the grid and thus the dimension
|
||||
of the index tuple to specify a grid */
|
||||
void init ( const SrArray<SrGridAxis>& axis_desc );
|
||||
|
||||
const SrArray<SrGridAxis>& axis_desc() const { return _axis; }
|
||||
|
||||
float max_coord ( int axis ) const { return _axis[axis].max; }
|
||||
float min_coord ( int axis ) const { return _axis[axis].min; }
|
||||
int segments ( int axis ) const { return _axis[axis].segs; }
|
||||
float seglen ( int axis ) const { return _seglen[axis]; }
|
||||
|
||||
/*! Returns the number of dimensions of the grid */
|
||||
int dimensions () const { return _axis.size(); }
|
||||
|
||||
/*! Returns the total number of cells in the grid */
|
||||
int cells () const { return _cells; }
|
||||
|
||||
/*! Get the index of a cell from its coordinates. 2D version. */
|
||||
int cell_index ( int i, int j ) const;
|
||||
|
||||
/*! Get the index of a cell from its coordinates. 3D version */
|
||||
int cell_index ( int i, int j, int k ) const;
|
||||
|
||||
/*! Get the index of a cell from its coordinates. Multidimensional version */
|
||||
int cell_index ( const SrArray<int>& coords ) const;
|
||||
|
||||
/*! Get the cell coordinates from its index. 2D version. */
|
||||
void cell_coords ( int index, int& i, int& j ) const;
|
||||
|
||||
/*! Get the cell coordinates from its index. 3D version. */
|
||||
void cell_coords ( int index, int& i, int& j, int& k ) const;
|
||||
|
||||
/*! Get the cell coordinates from its index. Multidimensional version. */
|
||||
void cell_coords ( int index, SrArray<int>& coords ) const;
|
||||
|
||||
/*! Get the lower and upper coordinates of the Euclidian 2D box of cell (i,j). */
|
||||
void cell_boundary ( int i, int j, SrPnt2& a, SrPnt2& b ) const;
|
||||
|
||||
/*! Get the lower and upper coordinates of the Euclidian 3D box of cell (i,j,k). */
|
||||
void cell_boundary ( int i, int j, int k, SrPnt& a, SrPnt& b ) const;
|
||||
|
||||
/*! Get the indices of all cells intersecting with the given box.
|
||||
If a box boundary is exactly at a grid separator, only the cell
|
||||
with greater index is considered. Indices are just pushed to
|
||||
array cells, which is not emptied before being used */
|
||||
void get_intersection ( SrPnt2 a, SrPnt2 b, SrArray<int>& cells ) const;
|
||||
|
||||
/*! 3D version of get_intersection() */
|
||||
void get_intersection ( SrPnt a, SrPnt b, SrArray<int>& cells ) const;
|
||||
|
||||
/*! Returns the index of the cell containing a, or -1 if a is outside the grid */
|
||||
int get_point_location ( SrPnt2 a ) const;
|
||||
|
||||
/*! Returns the index of the cell containing a, or -1 if a is outside the grid */
|
||||
int get_point_location ( SrPnt a ) const;
|
||||
};
|
||||
|
||||
/*! \class SrGrid sr_grid.h
|
||||
\brief n-D regular grid template class
|
||||
|
||||
SrGrid defines automatic type casts to the user type.
|
||||
WARNING: SrGrid is designed to efficiently store large
|
||||
amount of cells and it uses SrArray<X>, which implies
|
||||
that no constructors or destructors of type X are called.
|
||||
The user must initialize and delete data properly. */
|
||||
template <class X>
|
||||
class SrGrid : public SrGridBase
|
||||
{ private :
|
||||
SrArray<X> _data;
|
||||
|
||||
public :
|
||||
SrGrid ( int dim=0, int ns=0 ) : SrGridBase ( dim, ns )
|
||||
{ _data.size(SrGridBase::cells()); }
|
||||
|
||||
void init ( int dim, int ns )
|
||||
{ SrGridBase::init ( dim, ns );
|
||||
_data.size(SrGridBase::cells());
|
||||
}
|
||||
|
||||
void init ( const SrArray<SrGridAxis>& axis_desc )
|
||||
{ SrGridBase::init ( axis_desc );
|
||||
_data.size(SrGridBase::cells());
|
||||
}
|
||||
|
||||
/*! Returns the cell of given index, that should be in 0<=i<cells() */
|
||||
X& operator[] ( int index ) { return _data[index]; }
|
||||
|
||||
/*! Returns the cell of given 2D coordinates */
|
||||
X& operator() ( int i, int j )
|
||||
{ return _data[SrGridBase::cell_index(i,j)]; }
|
||||
|
||||
/*! Returns the cell of given 3D coordinates */
|
||||
X& operator() ( int i, int j, int k )
|
||||
{ return _data[SrGridBase::cell_index(i,j,k)]; }
|
||||
|
||||
/*! Returns the cell of given n-D coordinates */
|
||||
X& operator() ( const SrArray<int>& coords )
|
||||
{ return _data[SrGridBase::cell_index(i,j)]; }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_LIST_H
|
||||
|
||||
|
||||
242
source/dcdt/se/sr_hash_table.cpp
Normal file
242
source/dcdt/se/sr_hash_table.cpp
Normal file
@ -0,0 +1,242 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_hash_table.h"
|
||||
|
||||
//================================ hash function =============================
|
||||
|
||||
static int hash ( const char* s, int size )
|
||||
{
|
||||
int h = 0;
|
||||
while ( *s )
|
||||
{ h = 31*h + SR_LOWER(*s);
|
||||
s++;
|
||||
}
|
||||
h = SR_ABS(h);
|
||||
h = h%size;
|
||||
return h;
|
||||
}
|
||||
|
||||
/*static int hash ( const char *string, int limit ) // worse
|
||||
{
|
||||
unsigned int i = 0;
|
||||
unsigned int accum = 0;
|
||||
|
||||
while( string[ i ] != 0 ) {
|
||||
accum = string[ i ]
|
||||
^ ( accum << ( sizeof( char ) * 8 + 1 ) )
|
||||
^ ( accum >> ( ( sizeof( accum ) - sizeof( char ) ) * 8 - 1 ) );
|
||||
i++;
|
||||
}
|
||||
|
||||
return (accum % limit);
|
||||
}*/
|
||||
|
||||
//================================ SrHashTableBase ===============================
|
||||
|
||||
SrHashTableBase::SrHashTableBase ( int hsize )
|
||||
{
|
||||
init ( hsize );
|
||||
}
|
||||
|
||||
SrHashTableBase::~SrHashTableBase ()
|
||||
{
|
||||
init ( 0 );
|
||||
}
|
||||
|
||||
void SrHashTableBase::init ( int hsize )
|
||||
{
|
||||
if ( hsize==0 && _hash_size==0 ) return; // already initialized to 0
|
||||
|
||||
// destroys actual table:
|
||||
while ( _table.size() )
|
||||
{ if ( _table.top().dynamic ) sr_string_set ( _table.top().st, 0 ); // delete
|
||||
_table.pop();
|
||||
}
|
||||
|
||||
// builds new table:
|
||||
_free.size ( 0 );
|
||||
_free.capacity ( 0 );
|
||||
_last_id = -1;
|
||||
_elements = 0;
|
||||
_hash_size = hsize;
|
||||
_table.capacity ( hsize );
|
||||
_table.size ( hsize );
|
||||
int i;
|
||||
for ( i=0; i<hsize; i++ )
|
||||
_set_entry ( i, 0/*st*/, 0/*data*/, 0/*dynamic*/ );
|
||||
}
|
||||
|
||||
void SrHashTableBase::rehash ( int new_hsize )
|
||||
{
|
||||
SrArray<Entry> a;
|
||||
a.capacity ( _elements );
|
||||
a.size ( 0 );
|
||||
|
||||
// 1. store current valid elements:
|
||||
int i;
|
||||
for ( i=0; i<_table.size(); i++ )
|
||||
{ if ( _table[i].st )
|
||||
{ a.push() = _table[i];
|
||||
_table[i].st=0; // ensures st pointer will not be deleted
|
||||
}
|
||||
}
|
||||
|
||||
// 2. init table with new hash size:
|
||||
init ( new_hsize );
|
||||
|
||||
// 3. put back original data in new table:
|
||||
for ( i=0; i<a.size(); i++ )
|
||||
{ _insert ( a[i].st, a[i].data, a[i].dynamic );
|
||||
}
|
||||
}
|
||||
|
||||
int SrHashTableBase::longest_entry () const
|
||||
{
|
||||
int i, j, len;
|
||||
int longest=0;
|
||||
|
||||
for ( i=0; i<_hash_size; i++ )
|
||||
{
|
||||
if ( _table[i].st==0 ) continue;
|
||||
|
||||
len = 1;
|
||||
j=_table[i].next;
|
||||
while ( j>=0 )
|
||||
{ len++;
|
||||
j = _table[j].next;
|
||||
}
|
||||
|
||||
if ( len>longest ) longest=len;
|
||||
|
||||
}
|
||||
|
||||
return longest;
|
||||
}
|
||||
|
||||
int SrHashTableBase::lookup_index ( const char *st ) const
|
||||
{
|
||||
if ( !st ) return -1;
|
||||
|
||||
int id = ::hash ( st, _hash_size );
|
||||
|
||||
if ( _table[id].st==0 ) return -1; // empty entry, not found
|
||||
|
||||
while ( true )
|
||||
{ if ( sr_compare(_table[id].st,st)==0 ) return id; // already there
|
||||
|
||||
// else check next colliding entry:
|
||||
if ( _table[id].next<0 ) return -1; // no more entries, not found
|
||||
id = _table[id].next;
|
||||
}
|
||||
}
|
||||
|
||||
void* SrHashTableBase::lookup ( const char* st ) const
|
||||
{
|
||||
int id = lookup_index ( st );
|
||||
return id<0? 0: _table[id].data;
|
||||
}
|
||||
|
||||
bool SrHashTableBase::_insert ( const char *st, void* data, char dynamic )
|
||||
{
|
||||
if ( !st ) { _last_id=-1; return false; }
|
||||
if ( _hash_size<=0 ) init ( 256 ); // automatic initialization to avoid problems
|
||||
|
||||
int id = ::hash ( st, _hash_size );
|
||||
|
||||
if ( _table[id].st==0 ) // empty entry, just take it
|
||||
{ _set_entry ( id, st, data, dynamic );
|
||||
_elements++;
|
||||
_last_id = id;
|
||||
return true;
|
||||
}
|
||||
|
||||
while ( true )
|
||||
{ if ( sr_compare(_table[id].st,st)==0 ) // already there
|
||||
{ _last_id = id;
|
||||
return false;
|
||||
}
|
||||
|
||||
// else check next colliding entry:
|
||||
if ( _table[id].next<0 ) // no more entries, add one:
|
||||
{ int newid;
|
||||
if ( _free.size()>0 )
|
||||
{ newid = _free.pop();
|
||||
}
|
||||
else
|
||||
{ newid = _table.size();
|
||||
_table.push();
|
||||
}
|
||||
_table[id].next = newid;
|
||||
_set_entry ( newid, st, data, dynamic );
|
||||
_elements++;
|
||||
_last_id = newid;
|
||||
return true;
|
||||
}
|
||||
|
||||
id = _table[id].next;
|
||||
}
|
||||
}
|
||||
|
||||
void* SrHashTableBase::remove ( const char *st )
|
||||
{
|
||||
if ( !st ) { _last_id=-1; return false; }
|
||||
|
||||
int id = ::hash ( st, _hash_size );
|
||||
|
||||
if ( _table[id].st==0 ) return 0; // already empty entry
|
||||
|
||||
int priorid=id;
|
||||
while ( true )
|
||||
{ if ( sr_compare(_table[id].st,st)==0 ) // found: remove it
|
||||
{
|
||||
void* data = _table[id].data;
|
||||
int next = _table[id].next;
|
||||
_clear_entry ( id );
|
||||
_elements--;
|
||||
|
||||
// fix links:
|
||||
if ( priorid==id ) // removing first entry
|
||||
{ if ( next>=0 )
|
||||
{ _table[id] = _table[next];
|
||||
_set_entry ( next, 0 /*st*/, 0 /*data*/, 0 /*dynamic*/ );
|
||||
_free.push() = next;
|
||||
}
|
||||
else
|
||||
{ } // nothing to do
|
||||
}
|
||||
else // removing entry in the "linked list"
|
||||
{ _table[priorid].next = next;
|
||||
_free.push() = id;
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
priorid = id;
|
||||
id = _table[id].next;
|
||||
}
|
||||
}
|
||||
|
||||
void SrHashTableBase::_set_entry ( int id, const char *st, void* data, char dynamic )
|
||||
{
|
||||
if ( dynamic )
|
||||
_table[id].st = sr_string_new ( st ); // can return null if st==0
|
||||
else
|
||||
_table[id].st = (char*)st;
|
||||
|
||||
_table[id].data = data;
|
||||
_table[id].next = -1;
|
||||
_table[id].dynamic = dynamic;
|
||||
}
|
||||
|
||||
void SrHashTableBase::_clear_entry ( int id )
|
||||
{
|
||||
if ( _table[id].dynamic ) delete _table[id].st;
|
||||
|
||||
_table[id].st = 0;
|
||||
_table[id].data = 0;
|
||||
_table[id].next = -1;
|
||||
_table[id].dynamic = 0;
|
||||
}
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
155
source/dcdt/se/sr_hash_table.h
Normal file
155
source/dcdt/se/sr_hash_table.h
Normal file
@ -0,0 +1,155 @@
|
||||
|
||||
/** \file sr_hash_table.h
|
||||
* Hash table functions */
|
||||
|
||||
# ifndef SR_HASH_TABLE_H
|
||||
# define SR_HASH_TABLE_H
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
//================================ SrHashTableBase ===============================
|
||||
|
||||
/*! \class SrHashTableBase sr_hash_table.h
|
||||
Stores user data associated with string keys in a hash table.
|
||||
Note1: this is a rather specific implementation: colliding elements
|
||||
are appended to the same array of entries, this is very efficient for inserting
|
||||
elements but less practical for removing elements (removal was not implemented).
|
||||
Note2: the user is responsible for allocation/deallocation of the appended user data,
|
||||
which is merely stored as given void pointers, more info about that in the SrHashTable class. */
|
||||
class SrHashTableBase
|
||||
{ protected:
|
||||
struct Entry { char* st; // the string id of this entry, or null if empty entry
|
||||
void* data; // the user data associated or null if none
|
||||
int next; // the index of the next colliding item
|
||||
char dynamic; // 1 if st was created with new operator, 0 if st is static
|
||||
};
|
||||
|
||||
SrArray<Entry> _table;
|
||||
SrArray<int> _free;
|
||||
int _hash_size;
|
||||
int _elements;
|
||||
int _last_id;
|
||||
|
||||
public:
|
||||
|
||||
/*! Default constructor creates a table of given hash size. If the hash size
|
||||
is 0 (the default value), init() must be properly called aftwerwards. */
|
||||
SrHashTableBase ( int hsize=0 );
|
||||
|
||||
/*! Destructor */
|
||||
~SrHashTableBase ();
|
||||
|
||||
/*! Destroy actual table and builds a new empty one with the given hash size.
|
||||
A value of zero will make the hash table empty and unusable. */
|
||||
void init ( int hsize );
|
||||
|
||||
/*! Rebuilds the table with a different hash size */
|
||||
void rehash ( int new_hsize );
|
||||
|
||||
/*! Returns the table size, which can be greater than the initial
|
||||
given hash size in case there were collisions */
|
||||
int size () const { return _table.size(); }
|
||||
|
||||
/*! Returns the initial (minimum) size of the table */
|
||||
int hash_size () const { return _hash_size; }
|
||||
|
||||
/*! Returns the number of colliding elements in the table */
|
||||
int collisions () const { return _table.size()-_free.size()-_hash_size; }
|
||||
|
||||
/*! Calculates and returns the maximum number of comparisons
|
||||
that can happen when searching for a string in the table,
|
||||
ie, the longest list size associated with a same entry in
|
||||
the hash table. If there are no collisions, 1 is returned. */
|
||||
int longest_entry () const;
|
||||
|
||||
/*! Total number of elements inserted in the table */
|
||||
int elements () const { return _elements; }
|
||||
|
||||
/*! Returns the next index of an entry colliding with index id.
|
||||
-1 is returned in case there is no further "colliding index" */
|
||||
int collidingid ( int id ) const { return _table[id].next; }
|
||||
|
||||
/*! Returns the string key associated with the given id (can be null).
|
||||
No validity checkings in the index are done! */
|
||||
const char* key ( int id ) const { return _table[id].st; }
|
||||
|
||||
/*! Returns the user data associated with the given id (can be null).
|
||||
No validity checkings in the index are done! */
|
||||
void* data ( int id ) const { return _table[id].data; }
|
||||
|
||||
/*! Returns the valid index entry (>=0) relative to the given string key,
|
||||
or -1 if the string key does not exist in the table (or if st==0)*/
|
||||
int lookup_index ( const char *st ) const;
|
||||
|
||||
/*! Returns the user data associated with the given string key,
|
||||
or null if the string was not found */
|
||||
void* lookup ( const char* st ) const;
|
||||
|
||||
/*! Inserts a string key and user data to the table and returns true in case of success.
|
||||
False is returned in case the string key already exists (or if st==0) meaning
|
||||
that the entry was not added to the table.
|
||||
If the string already exists, its index can be retrieved with lastid().
|
||||
The given st pointer is copied in an internal allocated string.
|
||||
(note: if not set, the hash size is automatically initialized with 256) */
|
||||
bool insert ( const char *st, void* data ) { return _insert(st,data,1); }
|
||||
|
||||
/*! Same as the insert() method, however the given st pointer is considered to
|
||||
be a pointer to a static string and is not internally allocated. */
|
||||
bool insertstat ( const char *st, void* data ) { return _insert(st,data,0); }
|
||||
|
||||
/*! Returns the id involved during the last call to the insert methods:
|
||||
it will be 1) the index of the new added entry, or 2) the index of the found
|
||||
duplicated entry, or 3) -1 is the string key was null. */
|
||||
int lastid () const { return _last_id; }
|
||||
|
||||
/*! Removes and returns the data associated with key st. Returns 0 if
|
||||
the key was not found. */
|
||||
void* remove ( const char *st );
|
||||
|
||||
private :
|
||||
bool _insert ( const char *st, void* data, char dynamic );
|
||||
void _set_entry ( int id, const char *st, void* data, char dynamic );
|
||||
void _clear_entry ( int id );
|
||||
};
|
||||
|
||||
//================================ SrHasTable ===============================
|
||||
|
||||
/*! \class SrHasTable sr_hash_table.h
|
||||
Template version for usefull typecasts to a user type X.
|
||||
For generality, there is no destructor and the user is responsible to
|
||||
delete any attached pointers, using or not methods delete_data() and unref_data(). */
|
||||
template <class X>
|
||||
class SrHashTable : public SrHashTableBase
|
||||
{ public:
|
||||
/*! This constructor simply calls the constructor of the base class */
|
||||
SrHashTable ( int hsize ) : SrHashTableBase(hsize) {}
|
||||
|
||||
/*! Simple type cast to the base class method */
|
||||
X data ( int id ) const { return (X)SrHashTableBase::data(id); }
|
||||
|
||||
/*! Simple type cast to the base class method */
|
||||
X lookup ( const char* st ) const { return (X)SrHashTableBase::lookup(st); }
|
||||
|
||||
/*! Will delete and set to zero all attached data in the hash table using
|
||||
the delete operator after a type cast to (X*). */
|
||||
void delete_data ()
|
||||
{ int i;
|
||||
for (i=0; i<_table.size(); i++)
|
||||
{ delete (X*)_table[i].data; _table[i].data=0; } // delete 0 is ok
|
||||
}
|
||||
|
||||
/*! Will call unref() and set to zero all attached data in the hash table. It assumes
|
||||
that X derives SrSharedClass, and therefore X* is type-casted to SrSharedClass*. */
|
||||
void unref_data ()
|
||||
{ int i;
|
||||
for (i=0; i<_table.size(); i++)
|
||||
{ if (_table[i].data) { ((SrSharedClass*)_table[i].data)->unref(); _table[i].data=0; } }
|
||||
}
|
||||
|
||||
/*! Simple type cast to the base class method */
|
||||
X remove ( const char *st ) { return (X)SrHashTableBase::remove(st); }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_HASH_TABLE_H
|
||||
119
source/dcdt/se/sr_heap.h
Normal file
119
source/dcdt/se/sr_heap.h
Normal file
@ -0,0 +1,119 @@
|
||||
|
||||
# ifndef SR_HEAP_H
|
||||
# define SR_HEAP_H
|
||||
|
||||
/** \file sr_heap.h
|
||||
* Template for a heap based on SrArray. */
|
||||
|
||||
# include "sr_array.h"
|
||||
|
||||
/*! \class SrHeap sr_heap.h
|
||||
\brief Heap based on a SrArray
|
||||
|
||||
SrHeap implements a heap ordered binary tree based on a SrArray object.
|
||||
Because of performance issues, SrHeap does not honor constructors or
|
||||
destructors of its elements X, so user-managed pointers should be used
|
||||
in case of more complex classes, in the same design line as SrArray.
|
||||
The heap is ordered according to its associated cost. The remove()
|
||||
method will always remove the minimum cost element in the heap.
|
||||
class X is the element type, and Y is the cost type. Y will usually
|
||||
be int, float or double. */
|
||||
template <class X, class Y>
|
||||
class SrHeap
|
||||
{ private :
|
||||
struct Elem { X e; Y c; };
|
||||
SrArray<Elem> _heap;
|
||||
|
||||
public :
|
||||
|
||||
/*! Default constructor. */
|
||||
SrHeap () {}
|
||||
|
||||
/*! Copy constructor. */
|
||||
SrHeap ( const SrHeap& h ) : _heap(h._heap) {}
|
||||
|
||||
/*! Set the capacity of the internal array */
|
||||
void capacity ( int c ) { _heap.capacity(c); }
|
||||
|
||||
/*! Returns true if the heap is empty, false otherwise. */
|
||||
bool empty () const { return _heap.empty(); }
|
||||
|
||||
/*! Returns the number of elements in the queue. */
|
||||
int size () const { return _heap.size(); }
|
||||
|
||||
/*! Initializes as an empty heap */
|
||||
void init () { _heap.size(0); }
|
||||
|
||||
/*! Compress the internal heap array */
|
||||
void compress () { _heap.compress(0); }
|
||||
|
||||
/*! Make the heap have the given size, by removing the worst elements.
|
||||
Only applicable when s < size(). */
|
||||
void size ( int s )
|
||||
{ if ( s<=0 ) { init(); return; }
|
||||
if ( s>=size() ) return;
|
||||
SrArray<Elem> tmp(s,s);
|
||||
while ( tmp.size()<s )
|
||||
{ tmp.push() = _heap.top();
|
||||
remove();
|
||||
}
|
||||
init();
|
||||
while ( tmp.size()>0 )
|
||||
{ insert ( tmp.top().e, tmp.top().c );
|
||||
tmp.pop();
|
||||
}
|
||||
}
|
||||
|
||||
/*! Insert a new element with the given cost */
|
||||
void insert ( const X& elem, Y cost )
|
||||
{ // insert at the end:
|
||||
_heap.push();
|
||||
_heap.top().e = elem;
|
||||
_heap.top().c = cost;
|
||||
// swim up: (parent of elem k is k/2)
|
||||
Elem tmp;
|
||||
int k=_heap.size();
|
||||
while ( k>1 && _heap[k/2-1].c>_heap[k-1].c )
|
||||
{ SR_SWAP ( _heap[k/2-1], _heap[k-1] );
|
||||
k = k/2;
|
||||
}
|
||||
}
|
||||
|
||||
/*! Removes the element in the top of the heap, which is always
|
||||
the element with lowest cost. */
|
||||
void remove ()
|
||||
{ // put last element in top:
|
||||
_heap[0] = _heap.pop();
|
||||
// sink down: (children of node k are 2k and 2k+1)
|
||||
int j;
|
||||
int k=1;
|
||||
int n=_heap.size();
|
||||
Elem tmp;
|
||||
while ( 2*k<=n )
|
||||
{ j=2*k;
|
||||
if ( j<n && _heap[j-1].c>_heap[j].c ) j++;
|
||||
if ( !(_heap[k-1].c>_heap[j-1].c) ) break;
|
||||
SR_SWAP ( _heap[k-1], _heap[j-1] );
|
||||
k=j;
|
||||
}
|
||||
}
|
||||
|
||||
/*! Get a reference to the top element of the the heap,
|
||||
which is always the element with lowest cost. */
|
||||
const X& top () const { return _heap[0].e; }
|
||||
|
||||
/*! Get the lowest cost in the heap,
|
||||
which is always the cost of the top element. */
|
||||
Y lowest_cost () const { return _heap[0].c; }
|
||||
|
||||
/*! Returns elem i (0<=i<size) for inspection */
|
||||
const X& elem ( int i ) const { return _heap[i].e; }
|
||||
|
||||
/*! Returns the cost of elem i (0<=i<size) for inspection */
|
||||
Y cost ( int i ) const { return _heap[i].c; }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
#endif // SR_HEAP_H
|
||||
|
||||
116
source/dcdt/se/sr_image.cpp
Normal file
116
source/dcdt/se/sr_image.cpp
Normal file
@ -0,0 +1,116 @@
|
||||
#include "precompiled.h"
|
||||
# include <string.h>
|
||||
|
||||
# include "sr_image.h"
|
||||
|
||||
//=========================== SrImage ============================
|
||||
|
||||
SrImage::SrImage ()
|
||||
{
|
||||
_w = _h = _tw = 0;
|
||||
_data = 0;
|
||||
}
|
||||
|
||||
SrImage::~SrImage ()
|
||||
{
|
||||
delete _data;
|
||||
}
|
||||
|
||||
void SrImage::alloc ( int w, int h )
|
||||
{
|
||||
int newsize = w*h*3;
|
||||
|
||||
if ( newsize!=_tw*_h )
|
||||
{
|
||||
delete _data;
|
||||
|
||||
if ( w<=0 || h<=0 ) w=h=0;
|
||||
|
||||
if ( w )
|
||||
_data = new srbyte [ newsize ]; // to hold rgb values
|
||||
else
|
||||
_data = 0;
|
||||
}
|
||||
|
||||
_tw = 3*w;
|
||||
_w = w;
|
||||
_h = h;
|
||||
}
|
||||
|
||||
void SrImage::vertical_mirror ()
|
||||
{
|
||||
int i, ie, mid;
|
||||
mid = _h/2;
|
||||
|
||||
srbyte* buf = new srbyte [ _tw ];
|
||||
|
||||
for ( i=0,ie=_h-1; i<mid; i++,ie-- )
|
||||
{ memcpy ( buf, line(i), _tw );
|
||||
memcpy ( line(i), line(ie), _tw );
|
||||
memcpy ( line(ie), buf, _tw );
|
||||
}
|
||||
|
||||
delete buf;
|
||||
}
|
||||
|
||||
# define PutInt(i) fwrite(&i,4/*bytes*/,1/*num items*/,f)
|
||||
# define PutShort(s) fwrite(&s,2/*bytes*/,1/*num items*/,f)
|
||||
|
||||
bool SrImage::save_as_bmp ( const char* filename )
|
||||
{
|
||||
FILE* f = fopen ( filename, "wb" );
|
||||
if ( !f ) return false;
|
||||
|
||||
int i = 0;
|
||||
int offset = 14+40;
|
||||
//int dw = 4-(_w%4); if ( dw==4 ) dw=0;
|
||||
int dw = (_w%4);
|
||||
|
||||
int filesize = 14 /*header*/ + 40 /*info*/ + (_w*_h*3) +(_h*dw);
|
||||
|
||||
// 14 bytes of header:
|
||||
fprintf ( f, "BM" ); // 2 bytes : signature
|
||||
PutInt ( filesize ); // file size
|
||||
PutInt ( i ); // reserved (zeros)
|
||||
PutInt ( offset ); // offset to the data
|
||||
|
||||
// 40 bytes of info header:
|
||||
int infosize = 40;
|
||||
short planes = 1;
|
||||
short bits = 24;
|
||||
int compression = 0; // no compression
|
||||
int compsize = 0; // no compression
|
||||
int hres = 600;
|
||||
int vres = 600;
|
||||
int colors = 0;
|
||||
int impcolors = 0; // important colors: all
|
||||
PutInt ( infosize ); // size of info header
|
||||
PutInt ( _w ); // width
|
||||
PutInt ( _h ); // height
|
||||
PutShort ( planes );
|
||||
PutShort ( bits );
|
||||
PutInt ( compression );
|
||||
PutInt ( compsize );
|
||||
PutInt ( hres );
|
||||
PutInt ( vres );
|
||||
PutInt ( colors );
|
||||
PutInt ( impcolors );
|
||||
|
||||
int w, h;
|
||||
srbyte* scanline;
|
||||
for ( h=_h-1; h>=0; h-- )
|
||||
{ scanline = line(h);
|
||||
for ( w=0; w<_tw; w+=3 )
|
||||
{ fputc ( scanline[w+2], f ); // B
|
||||
fputc ( scanline[w+1], f ); // G
|
||||
fputc ( scanline[w], f ); // R
|
||||
}
|
||||
for ( w=0; w<dw; w++ ) fputc ( 0, f );
|
||||
}
|
||||
|
||||
fclose ( f );
|
||||
return true;
|
||||
}
|
||||
|
||||
//============================= end of file ==========================
|
||||
|
||||
67
source/dcdt/se/sr_image.h
Normal file
67
source/dcdt/se/sr_image.h
Normal file
@ -0,0 +1,67 @@
|
||||
|
||||
# ifndef SR_IMAGE_H
|
||||
# define SR_IMAGE_H
|
||||
|
||||
/** \file sr_image.h
|
||||
* A 24 bit image.*/
|
||||
|
||||
# include "sr_color.h"
|
||||
|
||||
//=========================== SrImage ============================
|
||||
|
||||
/*! \class SrImage sr_image.h
|
||||
\brief Non compressed 24 bit image
|
||||
SrImage stores pixel data as a sequence of rgb data. */
|
||||
class SrImage
|
||||
{ private :
|
||||
int _w, _h; // total data size is 3*_w*_h
|
||||
int _tw; // _tw = 3*_w
|
||||
srbyte* _data; // array of rgb values
|
||||
|
||||
public :
|
||||
|
||||
/*! Constructs an empty image */
|
||||
SrImage ();
|
||||
|
||||
/*! Destructor */
|
||||
~SrImage ();
|
||||
|
||||
/*! Alloc the desired size in pixels of the image.
|
||||
A total of w*h*3 elements are allocated.
|
||||
Invalid dimensions deletes the image data */
|
||||
void alloc ( int w, int h );
|
||||
|
||||
/*! Changes the image by appying a vertical mirroring */
|
||||
void vertical_mirror ();
|
||||
|
||||
/*! Returns the width in pixels of the image */
|
||||
int w () const { return _w; }
|
||||
|
||||
/*! Returns the height in pixels of the image */
|
||||
int h () const { return _h; }
|
||||
|
||||
/*! Returns a pointer to the base image data */
|
||||
const srbyte* data () { return _data; }
|
||||
|
||||
/*! Returns a pointer to the pixel color (3 bytes) at position (l,c) */
|
||||
srbyte* data ( int l, int c ) { return _data+(l*_tw)+(c*3); }
|
||||
|
||||
/*! Returns a reference to the red component of the pixel at position (l,c) */
|
||||
srbyte& r ( int l, int c ) { return data(l,c)[0]; }
|
||||
|
||||
/*! Returns a reference to the green component of the pixel at position (l,c) */
|
||||
srbyte& g ( int l, int c ) { return data(l,c)[1]; }
|
||||
|
||||
/*! Returns a reference to the blue component of the pixel at position (l,c) */
|
||||
srbyte& b ( int l, int c ) { return data(l,c)[2]; }
|
||||
|
||||
/*! Returns the base pointer of the line l of the image */
|
||||
srbyte* line ( int l ) { return _data+(l*_tw); }
|
||||
|
||||
/*! Saves the image in a bmp file. Returns true if could write file and false otherwise. */
|
||||
bool save_as_bmp ( const char* filename );
|
||||
};
|
||||
|
||||
//============================= end of file ==========================
|
||||
|
||||
# endif // SR_IMAGE_H
|
||||
606
source/dcdt/se/sr_input.cpp
Normal file
606
source/dcdt/se/sr_input.cpp
Normal file
@ -0,0 +1,606 @@
|
||||
#include "precompiled.h"
|
||||
# include <stdlib.h>
|
||||
# include <string.h>
|
||||
# include <ctype.h>
|
||||
|
||||
# include "sr_input.h"
|
||||
# include "sr_string.h"
|
||||
# include "sr_array.h"
|
||||
|
||||
//# define SR_USE_TRACE1 //Parser
|
||||
//# define SR_USE_TRACE2 //Init
|
||||
# include "sr_trace.h"
|
||||
|
||||
# define ISNULL _type==(srbyte)TypeNull
|
||||
# define ISFILE _type==(srbyte)TypeFile
|
||||
# define ISSTRING _type==(srbyte)TypeString
|
||||
|
||||
//=============================== SrInput =================================
|
||||
|
||||
struct SrInput::UngetData
|
||||
{ struct Token { char* string; srbyte type; };
|
||||
SrArray<int> character; // unget buffer for char reading
|
||||
SrArray<Token> token; // unget buffer for token reading
|
||||
void init ();
|
||||
};
|
||||
|
||||
void SrInput::UngetData::init ()
|
||||
{
|
||||
character.size(0);
|
||||
while ( token.size() ) delete token.pop().string;
|
||||
}
|
||||
|
||||
void SrInput::_init ( char c )
|
||||
{
|
||||
_size = 0;
|
||||
_curline = 0;
|
||||
_comment_style = c;
|
||||
_type = (srbyte) TypeNull;
|
||||
_last_error = (srbyte) NoError;
|
||||
_last_token_type = 0;
|
||||
_max_token_size = 256;
|
||||
_lowercase_tokens = 1; // true
|
||||
_skipped_spaces = 0;
|
||||
_unget = new UngetData;
|
||||
_filename = 0;
|
||||
}
|
||||
|
||||
SrInput::SrInput ( char com )
|
||||
{
|
||||
SR_TRACE2 ("Default Constructor");
|
||||
_init ( com );
|
||||
}
|
||||
|
||||
SrInput::SrInput ( const char *buff, char com )
|
||||
{
|
||||
SR_TRACE2 ("String Constructor");
|
||||
_init ( com );
|
||||
if ( buff )
|
||||
{ _cur.s = buff;
|
||||
_ini.s = buff;
|
||||
_size = strlen ( buff );
|
||||
_curline = 1;
|
||||
_type = (srbyte) TypeString;
|
||||
}
|
||||
}
|
||||
|
||||
// static utility function:
|
||||
static void get_size ( FILE *fp, int &size, int &start )
|
||||
{
|
||||
start = (int)ftell(fp);
|
||||
fseek ( fp, 0, SEEK_END );
|
||||
size = (int)ftell(fp) - start;
|
||||
fseek ( fp, start, SEEK_SET );
|
||||
}
|
||||
|
||||
SrInput::SrInput ( FILE *file, char com )
|
||||
{
|
||||
SR_TRACE2 ("File Constructor");
|
||||
_init ( com );
|
||||
if ( file )
|
||||
{ _cur.f = file;
|
||||
_curline = 1;
|
||||
_type = (srbyte) TypeFile;
|
||||
get_size ( file, _size, _ini.f );
|
||||
}
|
||||
}
|
||||
|
||||
SrInput::SrInput ( const char* filename, const char* mode, char com )
|
||||
{
|
||||
SR_TRACE2 ("File2 Constructor");
|
||||
_init ( com );
|
||||
FILE* file = fopen ( filename, mode );
|
||||
sr_string_set ( _filename, filename );
|
||||
if ( file )
|
||||
{ _cur.f = file;
|
||||
_curline = 1;
|
||||
_type = (srbyte) TypeFile;
|
||||
get_size ( file, _size, _ini.f );
|
||||
}
|
||||
}
|
||||
|
||||
SrInput::~SrInput ()
|
||||
{
|
||||
close (); // close frees all data inside _unget, and frees _filename
|
||||
delete _unget;
|
||||
}
|
||||
|
||||
void SrInput::init ( const char *buff )
|
||||
{
|
||||
SR_TRACE2 ("Init with string");
|
||||
close ();
|
||||
if ( buff )
|
||||
{ _cur.s = buff;
|
||||
_ini.s = buff;
|
||||
_size = strlen ( buff );
|
||||
_curline = 1;
|
||||
_type = (srbyte) TypeString;
|
||||
}
|
||||
}
|
||||
|
||||
void SrInput::init ( FILE *file )
|
||||
{
|
||||
SR_TRACE2 ("Init with file");
|
||||
close ();
|
||||
if ( file )
|
||||
{ _cur.f = file;
|
||||
_curline = 1;
|
||||
_type = (srbyte) TypeFile;
|
||||
get_size ( file, _size, _ini.f );
|
||||
}
|
||||
}
|
||||
|
||||
void SrInput::init ( const char* filename, const char* mode )
|
||||
{
|
||||
SR_TRACE2 ("Init with file2");
|
||||
FILE* file = fopen ( filename, mode );
|
||||
init ( file );
|
||||
sr_string_set ( _filename, filename );
|
||||
}
|
||||
|
||||
void SrInput::close ()
|
||||
{
|
||||
if ( ISFILE ) fclose ( _cur.f );
|
||||
_size = 0;
|
||||
_curline = 0;
|
||||
_type = (srbyte) TypeNull;
|
||||
_last_error = (srbyte) NoError;
|
||||
_last_token = "";
|
||||
_last_token_type = 0;
|
||||
_unget->init ();
|
||||
sr_string_set ( _filename, 0 );
|
||||
}
|
||||
|
||||
void SrInput::leave_file ()
|
||||
{
|
||||
_type = (srbyte) TypeNull;
|
||||
close ();
|
||||
}
|
||||
|
||||
FILE* SrInput::filept ()
|
||||
{
|
||||
return _type==TypeFile? _cur.f:0;
|
||||
}
|
||||
|
||||
bool SrInput::valid () const
|
||||
{
|
||||
return (ISNULL)? false : true;
|
||||
}
|
||||
|
||||
bool SrInput::finished ()
|
||||
{
|
||||
if ( _unget->character.size()>0 || _unget->token.size()>0 ) return false;
|
||||
else if ( ISFILE ) return pos()>=_size? true:false;
|
||||
else if ( ISSTRING ) return *(_cur.s)? false:true;
|
||||
else return true;
|
||||
}
|
||||
|
||||
void SrInput::getall ( SrString& buf )
|
||||
{
|
||||
if ( ISFILE )
|
||||
{ int s = size()-pos();
|
||||
buf.capacity ( s+2 ); // need +2 to cope with pc text files
|
||||
fread ( (void*)(const char*)buf, sizeof(char), (size_t)s, _cur.f );
|
||||
buf [ s+1 ] = 0;
|
||||
}
|
||||
else if ( ISSTRING )
|
||||
{
|
||||
buf.set ( _cur.s );
|
||||
_cur.s = _ini.s+_size;
|
||||
}
|
||||
}
|
||||
|
||||
int SrInput::getline ( SrString& buf )
|
||||
{
|
||||
int c;
|
||||
|
||||
buf.len(0);
|
||||
do { c = get();
|
||||
buf << (char)c;
|
||||
} while ( c!='\n' && c!=EOF );
|
||||
|
||||
return c;
|
||||
}
|
||||
|
||||
int SrInput::getchar ()
|
||||
{
|
||||
int c = EOF;
|
||||
|
||||
if ( _unget->character.size()>0 )
|
||||
{ c = _unget->character.pop();
|
||||
}
|
||||
else
|
||||
{ if ( ISFILE ) c=fgetc(_cur.f);
|
||||
else if ( ISSTRING ) c = *_cur.s? *_cur.s++:EOF;
|
||||
}
|
||||
|
||||
if ( c=='\n' ) _curline++;
|
||||
return c;
|
||||
}
|
||||
|
||||
static void skip_c_comment ( SrInput *p )
|
||||
{
|
||||
int c, d;
|
||||
while ( true )
|
||||
{ c = p->getchar();
|
||||
if ( c=='/' ) // for nested comments
|
||||
{ d = p->getchar();
|
||||
if ( d=='*' ) skip_c_comment(p); else p->unget(d);
|
||||
}
|
||||
else if ( c=='*' )
|
||||
{ d = p->getchar();
|
||||
if ( d=='/' ) return; else p->unget(d);
|
||||
}
|
||||
else if ( c<0 ) return; // EOF found in the middle of a comment will not cause an error.
|
||||
}
|
||||
}
|
||||
|
||||
int SrInput::get ()
|
||||
{
|
||||
int c = getchar();
|
||||
|
||||
if ( _comment_style==0 )
|
||||
{ return c;
|
||||
}
|
||||
else if ( _comment_style=='C' && c=='/' )
|
||||
{ int d = getchar();
|
||||
if ( d=='*' )
|
||||
{ skip_c_comment(this);
|
||||
return get();
|
||||
}
|
||||
else if ( d=='/' )
|
||||
{ skip_line ();
|
||||
return get();
|
||||
}
|
||||
else unget(d);
|
||||
}
|
||||
else if ( c==_comment_style )
|
||||
{ skip_line ();
|
||||
return get();
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
void SrInput::unget ( char c )
|
||||
{
|
||||
if ( c=='\n' && _curline>0 ) _curline--;
|
||||
_unget->character.push() = c;
|
||||
}
|
||||
|
||||
void SrInput::advance ( int n )
|
||||
{
|
||||
while ( n-->0 ) if ( getchar()<0 ) break;
|
||||
}
|
||||
|
||||
void SrInput::rewind ()
|
||||
{
|
||||
if ( ISNULL ) return;
|
||||
_unget->init();
|
||||
_curline = 1;
|
||||
if ( ISSTRING ) _cur.s = _ini.s;
|
||||
else fseek ( _cur.f, _ini.f, SEEK_SET );
|
||||
}
|
||||
|
||||
int SrInput::pos ()
|
||||
{
|
||||
if ( ISFILE ) return ((int)ftell(_cur.f)) - _ini.f;
|
||||
else if ( ISSTRING ) return ((sruint)_cur.s) - ((sruint)_ini.s);
|
||||
else return 0;
|
||||
}
|
||||
|
||||
void SrInput::pos ( int pos )
|
||||
{
|
||||
_unget->init();
|
||||
if ( ISFILE ) fseek ( _cur.f, pos+_ini.f, SEEK_SET );
|
||||
else if ( ISSTRING ) _cur.s = _ini.s+pos;
|
||||
}
|
||||
|
||||
void SrInput::set_pos_and_update_cur_line ( int pos )
|
||||
{
|
||||
_unget->init();
|
||||
rewind ();
|
||||
advance ( pos );
|
||||
}
|
||||
|
||||
void SrInput::skip_line ()
|
||||
{
|
||||
int c;
|
||||
do { c = getchar();
|
||||
} while ( c!=EOF && c!='\n' );
|
||||
}
|
||||
|
||||
void SrInput::unget_token ( const char *token, SrInput::TokenType type )
|
||||
{
|
||||
UngetData::Token& t = _unget->token.push();
|
||||
t.string = 0;
|
||||
t.type = (srbyte) type;
|
||||
sr_string_set ( t.string, token );
|
||||
}
|
||||
|
||||
void SrInput::unget_token ()
|
||||
{
|
||||
if ( _last_token_type!=EndOfFile )
|
||||
unget_token ( _last_token, (TokenType)_last_token_type );
|
||||
}
|
||||
|
||||
bool SrInput::has_unget_data () const
|
||||
{
|
||||
return _unget->token.size()==0 && _unget->character.size()==0? false:true;
|
||||
}
|
||||
|
||||
static char get_escape_char ( char c )
|
||||
{
|
||||
switch ( c )
|
||||
{ case 'n' : return '\n';
|
||||
case 't' : return '\t';
|
||||
case '\n': return 0; // Just skip line
|
||||
default : return c;
|
||||
}
|
||||
}
|
||||
|
||||
SrInput::TokenType SrInput::get_token ( SrString &buf )
|
||||
{
|
||||
# define UNGET(c) if(c>0)unget(c)
|
||||
|
||||
int i;
|
||||
int c = ' ';
|
||||
int size = _max_token_size;
|
||||
TokenType ret;
|
||||
|
||||
_last_error = (srbyte) NoError;
|
||||
|
||||
buf.capacity(size);
|
||||
|
||||
if ( _unget->token.size()>0 )
|
||||
{ UngetData::Token& t = _unget->token.pop();
|
||||
buf.set ( t.string );
|
||||
delete t.string;
|
||||
return (TokenType) t.type;
|
||||
}
|
||||
|
||||
buf[0] = buf[1] = 0;
|
||||
|
||||
_skipped_spaces = 0;
|
||||
while ( c && isspace(c) ) // skip initial spaces;
|
||||
{ c=get(); _skipped_spaces++; }
|
||||
|
||||
if ( c==EOF )
|
||||
{ SR_TRACE1 ( "Got the End Of File!" );
|
||||
ret = EndOfFile;
|
||||
}
|
||||
else if ( strchr(SR_INPUT_DELIMITERS,c) && c!='.' ) // '.' will be detected after checking a real
|
||||
{ buf[0]=c; ret=Delimiter;
|
||||
SR_TRACE1 ( "Got a Delimiter: "<<buf );
|
||||
}
|
||||
else if ( c=='"' )
|
||||
{ SR_TRACE1 ( "Quote found..." );
|
||||
i = 0;
|
||||
while ( true )
|
||||
{ c = getchar(); // Comments inside a string are not considered
|
||||
if ( !c || c=='"' ) break;
|
||||
if ( c=='\\' ) c = get_escape_char ( getchar() );
|
||||
if ( c ) buf[i++]=c;
|
||||
if ( i+1==size ) break;
|
||||
}
|
||||
buf[i]=0;
|
||||
if ( i+1==size ) { SR_TRACE1("Got an Error TooBig!"); ret=Error; _last_error=TooBig; }
|
||||
else if ( c ) { SR_TRACE1("Got a String: ["<<buf<<"] size="<<strlen(buf)); ret=String; }
|
||||
else { SR_TRACE1("Got an Error OpenString!"); ret=Error; _last_error=OpenString; }
|
||||
}
|
||||
else if ( c=='.' || isdigit(c) )
|
||||
{ SR_TRACE1 ( "Digit found..." );
|
||||
bool pnt=false, exp=false;
|
||||
i = 0;
|
||||
while ( true )
|
||||
{ if ( c=='e' ) c='E';
|
||||
if ( !pnt && c=='.' ) pnt=true;
|
||||
else if ( pnt && c=='.' ) break;
|
||||
else if ( !exp && c=='E' ) exp=pnt=true;
|
||||
else if ( (c=='+'||c=='-') && buf[i-1]=='E' );
|
||||
else if ( !isdigit(c) ) break;
|
||||
buf[i++]=c;
|
||||
if ( i+1==size ) break;
|
||||
c = getchar();
|
||||
}
|
||||
buf[i]=0;
|
||||
|
||||
if ( buf[0]=='.' && i==1 && strchr(SR_INPUT_DELIMITERS,'.') )
|
||||
{ ret=Delimiter; UNGET(c); SR_TRACE1("Got a Delimiter: ["<<buf<<"]"); }
|
||||
else if ( i+1==size ) { ret=Error; _last_error=TooBig; SR_TRACE1("Got an Error TooBig!"); }
|
||||
else if ( pnt && c=='.' ) { ret=Error; _last_error=InvalidPoint; SR_TRACE1("Got an Error InvalidPoint!"); }
|
||||
else if ( pnt || exp ) { ret=Real; UNGET(c); SR_TRACE1("Got a Real: ["<<buf<<"] => "<<atof(buf)); }
|
||||
else { ret=Integer; UNGET(c); SR_TRACE1("Got an Integer: ["<<buf<<"] => "<<atoi(buf)); }
|
||||
}
|
||||
else if ( c=='_' || isalpha(c) )
|
||||
{ SR_TRACE1 ( "Alpha found..." );
|
||||
i = 0;
|
||||
while ( true )
|
||||
{ if ( !isalnum(c) && c!='_' ) break;
|
||||
buf[i++]=c;
|
||||
if ( i+1==size ) break;
|
||||
c = getchar();
|
||||
}
|
||||
buf[i]=0;
|
||||
if ( i+1==size ) { SR_TRACE1("Got an Error TooBig!"); ret=Error; _last_error=TooBig; }
|
||||
else { SR_TRACE1("Got a Name: ["<<buf<<']'); ret=Name; UNGET(c); }
|
||||
if ( _lowercase_tokens ) buf.lower();
|
||||
}
|
||||
else
|
||||
{ SR_TRACE1("Got an Error Undef, code: "<<(int)c<<" ["<<(char)c<<']');
|
||||
buf[0]=c;
|
||||
ret = Error;
|
||||
_last_error = UndefChar;
|
||||
}
|
||||
|
||||
return ret;
|
||||
# undef UNGET
|
||||
}
|
||||
|
||||
SrInput::TokenType SrInput::get_token ()
|
||||
{
|
||||
_last_token_type = get_token ( _last_token );
|
||||
return (TokenType)_last_token_type;
|
||||
}
|
||||
|
||||
char* SrInput::error_desc ( SrInput::ErrorType t )
|
||||
{
|
||||
switch ( t )
|
||||
{ case UndefChar : return "Undefined character found";
|
||||
case UnexpectedToken: return "Parsed token is of an unexpected type";
|
||||
case TooBig : return "Name too big";
|
||||
case OpenString : return "EOF found before end of a string";
|
||||
case InvalidPoint : return "Misplaced decimal point";
|
||||
default : return 0;
|
||||
}
|
||||
}
|
||||
|
||||
SrString& SrInput::gets ()
|
||||
{
|
||||
get_token();
|
||||
if ( _last_token_type!=String && _last_token_type!=Name )
|
||||
{ _last_error=UnexpectedToken; _last_token=""; }
|
||||
return _last_token;
|
||||
}
|
||||
|
||||
SrString& SrInput::gets ( SrString& buf )
|
||||
{
|
||||
TokenType t = get_token ( buf );
|
||||
if ( t!=String && t!=Name ) _last_error=UnexpectedToken;
|
||||
return buf;
|
||||
}
|
||||
|
||||
SrString& SrInput::getn ()
|
||||
{
|
||||
int signal=1;
|
||||
|
||||
get_token();
|
||||
|
||||
// accumulate +- delimiter if any:
|
||||
while ( _last_token_type==Delimiter )
|
||||
{ if ( _last_token[0]=='-' )
|
||||
signal*=-1;
|
||||
else if ( _last_token[1]!='+' )
|
||||
break; // will then return ""
|
||||
get_token();
|
||||
}
|
||||
|
||||
if ( _last_token_type!=Integer && _last_token_type!=Real )
|
||||
{ _last_error=UnexpectedToken; _last_token=""; return _last_token; }
|
||||
|
||||
if ( signal==-1 ) _last_token.insert(0,"-");
|
||||
|
||||
return _last_token;
|
||||
}
|
||||
|
||||
char SrInput::getd ()
|
||||
{
|
||||
get_token();
|
||||
if ( _last_token_type!=Delimiter )
|
||||
{ _last_error = UnexpectedToken;
|
||||
return 0;
|
||||
}
|
||||
return _last_token[0];
|
||||
}
|
||||
|
||||
bool SrInput::skip ( int n )
|
||||
{
|
||||
while ( n-- )
|
||||
{ get_token();
|
||||
if ( _last_token_type==Error || _last_token_type==EndOfFile ) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrInput::skipto ( const char *name )
|
||||
{
|
||||
while ( true )
|
||||
{ get_token();
|
||||
if ( _last_token_type==Error || _last_token_type==EndOfFile ) return false;
|
||||
if ( _last_token_type==Name )
|
||||
{ if ( _last_token==name ) return true; }
|
||||
}
|
||||
}
|
||||
|
||||
bool SrInput::has_field ()
|
||||
{
|
||||
if ( getd()!='<' ) { unget_token(); return false; }
|
||||
if ( gets()=="" ) { unget_token(); unget_token(); return false; }
|
||||
char d = getd();
|
||||
unget_token();
|
||||
unget_token();
|
||||
unget_token();
|
||||
return d=='>'? true:false;
|
||||
}
|
||||
|
||||
bool SrInput::read_field ( SrString& name )
|
||||
{
|
||||
name = "";
|
||||
if ( getd()!='<' ) return false;
|
||||
name = gets();
|
||||
if ( name=="" ) return false;
|
||||
if ( getd()!='>' ) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrInput::close_field ( const SrString& name )
|
||||
{
|
||||
if ( getd()!='<' ) return false;
|
||||
if ( getd()!='/' ) return false;
|
||||
if ( gets()!=name ) return false;
|
||||
if ( getd()!='>' ) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrInput::skip_field ( const SrString& name )
|
||||
{
|
||||
while ( true )
|
||||
{ if ( close_field(name) ) return true;
|
||||
if ( _last_token_type==EndOfFile ) return false;
|
||||
}
|
||||
}
|
||||
|
||||
//================================= operators ==================================
|
||||
|
||||
SrInput& operator>> ( SrInput& in, int& i )
|
||||
{
|
||||
SrString& s = in.getn();
|
||||
i = s.atoi();
|
||||
return in;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, sruint& i )
|
||||
{
|
||||
SrString& s = in.getn();
|
||||
i = (sruint)s.atoi();
|
||||
return in;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, srbyte& c )
|
||||
{
|
||||
SrString& s = in.getn();
|
||||
c = (srbyte)s.atoi();
|
||||
return in;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, float& f )
|
||||
{
|
||||
SrString& s = in.getn();
|
||||
f = s.atof();
|
||||
return in;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, double& d )
|
||||
{
|
||||
SrString& s = in.getn();
|
||||
d = s.atod();
|
||||
return in;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, char* st )
|
||||
{
|
||||
SrString& s = in.gets();
|
||||
strcpy ( st, s );
|
||||
return in;
|
||||
}
|
||||
|
||||
//============================ End of File ==============================
|
||||
369
source/dcdt/se/sr_input.h
Normal file
369
source/dcdt/se/sr_input.h
Normal file
@ -0,0 +1,369 @@
|
||||
|
||||
# ifndef SR_INPUT_H
|
||||
# define SR_INPUT_H
|
||||
|
||||
/** \file sr_input.h
|
||||
* parses input file or string */
|
||||
|
||||
// sr_array.h cannot be included here because of circular references
|
||||
# include <stdio.h>
|
||||
# include "sr_string.h"
|
||||
|
||||
/*! Defines which characters are considered as delimiters. */
|
||||
# define SR_INPUT_DELIMITERS "{}[]();,.=+-/^*%:&|!~<>#?@\\\'"
|
||||
|
||||
/*! \class SrInput sr_input.h
|
||||
\brief Parses a file or string buffer
|
||||
|
||||
SrInput reads data from a string buffer or from an open file. It can
|
||||
be used to read data byte per byte, or by parsing tokens that are
|
||||
recognized as names, delimiters, integers, reals or strings.
|
||||
Comments can be specified to be skiped and error messages are returned. */
|
||||
class SrInput
|
||||
{ public :
|
||||
|
||||
/*! Indicates the type of the current input. It can be TypeFile, TypeString,
|
||||
or TypeNull. TypeNull indicates that the SrInput is not valid, and need
|
||||
to be connected to a file or string to become valid (by calling init()). */
|
||||
enum Type { TypeFile, //!< Input from a file
|
||||
TypeString, //!< Input from a string buffer
|
||||
TypeNull //!< Input not initialized, valid() will return false
|
||||
};
|
||||
|
||||
/*! Indicates the type of the token returned in get_token() */
|
||||
enum TokenType { Name, //!< A sequence of chars without spaces and delimiters
|
||||
Delimiter, //!< Any delimiter defined in macro SR_INPUT_DELIMITERS
|
||||
Integer, //!< An int type number
|
||||
Real, //!< A real type number
|
||||
String, //!< A seq inside "". Returns content converting c-like escape chars.
|
||||
EndOfFile, //!< End of file encountered
|
||||
Error //!< There was an error when parsing the last token.
|
||||
};
|
||||
|
||||
/*! Indicates the error occured in the last get operation. See the comments in each
|
||||
function to know the types of error that can be generated. */
|
||||
enum ErrorType { NoError, //!< No error occured in the last operation
|
||||
UndefChar, //!< Unrecognized char was found
|
||||
UnexpectedToken, //!< Parsed token is of an unexpected type
|
||||
TooBig, //!< token is bigger than the given buffer
|
||||
OpenString, //!< EOF found before end of a string
|
||||
InvalidPoint //!< Misplaced decimal point found in a real number
|
||||
};
|
||||
|
||||
private :
|
||||
|
||||
struct UngetData;
|
||||
union { FILE *f; const char *s; } _cur; // the current input position
|
||||
union { int f; const char *s; } _ini; // the beginning of the buffer
|
||||
int _size; // the size of the input in bytes
|
||||
int _curline; // keeps track of the current line
|
||||
char _comment_style; // 0:none, 'C':C/C++ like, otherwise skip line
|
||||
srbyte _type; // the enumerator Type
|
||||
srbyte _last_error; // last error, if any.
|
||||
SrString _last_token; // buffer with the last token read
|
||||
srbyte _last_token_type; // buffer with the last token type read
|
||||
int _max_token_size; // max size allowed for parsed tokens
|
||||
char _lowercase_tokens; // if tokens should be converted to lowercase
|
||||
int _skipped_spaces; // number of spaces skipped during last get_token
|
||||
UngetData* _unget; // unget char and token information
|
||||
char* _filename; // optional file name of the open file
|
||||
void _init ( char c );
|
||||
public :
|
||||
|
||||
/*! Construct an input of type SrInput::TypeNull.
|
||||
SrInput will only be operational when linked to a file or string by calling
|
||||
an init() function later. While SrInput is of type Null, the valid() method will
|
||||
return false. The parameter com is the comment style for this input and is 0 by
|
||||
default. See comment_style() for a description of possible styles. */
|
||||
SrInput ( char com=0 ); // init as a null input
|
||||
|
||||
/*! Construct an input of type SrInput::TypeString.
|
||||
In this way SrInput will read bytes from the given null-terminated string buffer.
|
||||
If buff is null, SrInput will be initialized as TypeNull, and not as a String type.
|
||||
Com is the comment style for this input and is 0 by default.
|
||||
See comment_style() for a description of possible styles. */
|
||||
SrInput ( const char *buff, char com=0 ); // init as a string input
|
||||
|
||||
/*! Construct an input of type SrInput::File
|
||||
In this way, SrInput will read bytes from the given C-style file stream. If the file pointer
|
||||
is null, SrInput will be initialized as TypeNull, and not as a File type. The current
|
||||
position pointed by file is considered to be the start, and so the size is measured
|
||||
since this given position. The parameter com is the comment style for this input and
|
||||
is 0 by default. See comment_style() for a description of possible styles */
|
||||
SrInput ( FILE *file, char com=0 ); // init as a file input
|
||||
|
||||
/*! Construct an input of type SrInput::File, from a given filename.
|
||||
This is similar to the previous constructor, but it receives the fopen() parameters
|
||||
directly (file name and mode strings) and stores internally the given file name. */
|
||||
SrInput ( const char* filename, const char* mode, char com=0 ); // init as a file input
|
||||
|
||||
/*! Closes the associated file if it is a TypeFile input.
|
||||
If it is required to leave the associated input file open, call leave_file() before. */
|
||||
~SrInput ();
|
||||
|
||||
/*! Closes actual input, and init it as TypeString.
|
||||
If buff is null, SrInput will be initialized as TypeNull, and not as a String type.
|
||||
The comment style is not changed, but unget data are freed. */
|
||||
void init ( const char *buff );
|
||||
|
||||
/*! Closes actual input, and init it as a File type.
|
||||
If file is null, SrInput will be initialized as TypeNull, and not as a File type.
|
||||
The comment style is not changed, but unget data are freed. */
|
||||
void init ( FILE *file );
|
||||
|
||||
/*! Closes actual input, and init it as a File type, opening given file name in given mode.
|
||||
If file could not be open, SrInput is initialized as TypeNull, and not as a File type.
|
||||
Filename is stored, the comment style is not changed, but unget data are freed. */
|
||||
void init ( const char* filename, const char* mode );
|
||||
|
||||
/*! Closes actual input and set it as TypeNull.
|
||||
If SrInput is of type file, the associated file is closed. In all cases,
|
||||
unget data is freed, filename is set to null, the last error is cleaned,
|
||||
and size and curline are set to zero. */
|
||||
void close ();
|
||||
|
||||
/*! Puts SrInput into TypeNull mode but without closing the current file.
|
||||
If SrInput is not of File type, the effect is the same as close(). */
|
||||
void leave_file ();
|
||||
|
||||
/*! If the input is done from a file, return the FILE pointer associated, otherwise
|
||||
will return 0 */
|
||||
FILE* filept ();
|
||||
|
||||
/*! Returns the file name used for opening a file input, or null if not available */
|
||||
const char* filename () const { return _filename; }
|
||||
|
||||
/*! Associates with the input a file name. The string is stored but not used by SrInput. */
|
||||
void filename ( const char* s ) { sr_string_set(_filename,s); }
|
||||
|
||||
/*! Returns true if the input is not of TypeNull type. */
|
||||
bool valid () const;
|
||||
|
||||
/*! Returns the type of the SrInput. */
|
||||
Type type () const { return (Type) _type; }
|
||||
|
||||
/*! Returns the size of the current input. Size is calculated as the number in bytes from
|
||||
the beginning of the input to the end of the input. The beginning is considered to be
|
||||
the pointer passed to the constructor or init() method, when it is calculated. */
|
||||
int size () const { return _size; }
|
||||
|
||||
/*! Returns the current line of the input. When SrInput is created or initialized, the
|
||||
current line is set to 1. Afterwards, the current line is updated internally each
|
||||
time a newline char is read. Note that when pos(int) method is used, the current
|
||||
line is no more valid. */
|
||||
int curline () const { return _curline; }
|
||||
|
||||
/*! Defines the comment style used for all "get methods", with exception to get_byte().
|
||||
The style can be set to 0 if no comments are allowed, can be set to 'C' to have
|
||||
standard C and C++ nested comments parsed, or otherwise when the specified
|
||||
character is encountered the whole line is skiped. */
|
||||
void comment_style ( char style ) { _comment_style=style; } // style can be 0,'C','anychar'
|
||||
|
||||
/*! Returns the current comment style being used. */
|
||||
char comment_style () const { return _comment_style; }
|
||||
|
||||
/*! Returns true if the current input is pointing after the end of the file. If the
|
||||
input source is empty but there is data to unget, the input is not considered finished.*/
|
||||
bool finished ();
|
||||
|
||||
/*! Gets all bytes of the input, starting from the current position, and put them
|
||||
in the given buffer. The input will point to its end. If input is of TypeNull,
|
||||
nothing is done. Note: the input is not parsed, just copied, therefore comments
|
||||
are also copied. */
|
||||
void getall ( SrString& buf );
|
||||
|
||||
/*! Gets all bytes of the current line, i.e. untill a '\n' is read,
|
||||
and put them in the given buffer (inlcuding the '\n').
|
||||
The input will point to the first byte after the new line character.
|
||||
The last character read is returned and will be either '\n' or EOF.
|
||||
Uses method get(), and therefore comments are parsed. */
|
||||
int getline ( SrString& buf );
|
||||
|
||||
/*! Get current character and advances pointer, will return -1 if end was reached.
|
||||
Will read next char from the associated string or file. If SrInput is of
|
||||
TypeNull, -1 is always returned. Comments are not considered, but unget
|
||||
chars are correctly handled. */
|
||||
int getchar (); // comments not handled
|
||||
|
||||
/*! Get current byte and advance, will return 0 if finish is reached.
|
||||
This is for ascii inputs where comments are skipped according to
|
||||
the current style. */
|
||||
int get (); // comments handled
|
||||
|
||||
/*! Puts a byte in the unget stack. */
|
||||
void unget ( char c );
|
||||
|
||||
/*! Reads the next n bytes of the current input.
|
||||
Note that unget data are considered, but comments are not. */
|
||||
void advance ( int n=1 );
|
||||
|
||||
/*! puts the pointer in the beginning of the input.
|
||||
Any unget data is cleared. Nothing is done if SrInput is of type Null. */
|
||||
void rewind ();
|
||||
|
||||
/*! Offset in bytes from the begining of the input. */
|
||||
int pos ();
|
||||
|
||||
/*! Puts the input pointer to the position pos, clearing all unget data.
|
||||
After calling it, curline() becomes invalid. Alternatively, method
|
||||
set_pos_and_update_cur_line() can be used */
|
||||
void pos ( int pos );
|
||||
|
||||
/*! Puts the input pointer to the position pos, clears all unget data and
|
||||
determines the line number using rewind() and advance(pos). */
|
||||
void set_pos_and_update_cur_line ( int pos );
|
||||
|
||||
/*! Reads all the current line (with getchar), stopping one byte after the
|
||||
first newline encountered. */
|
||||
void skip_line ();
|
||||
|
||||
/*! Sets the maximum allowed size for parsed tokens. Default is 256. */
|
||||
void max_token_size ( int s ) { _max_token_size=s; }
|
||||
|
||||
/*! Returns the current maximum allowed size for parsed tokens. */
|
||||
int max_token_size () const { return _max_token_size; }
|
||||
|
||||
/*! Determines if parsed tokens are transformed to lowercase or not. Default is true. */
|
||||
void lowercase_tokens ( bool b ) { _lowercase_tokens = b; }
|
||||
|
||||
/*! Retrieves the state if parsed tokens are transformed to lowercase or not. */
|
||||
bool lowercase_tokens () const { return _lowercase_tokens? true:false; }
|
||||
|
||||
/*! Returns the number of leading spaces skipped during the
|
||||
last call to get_token() */
|
||||
int skipped_spaces () const { return _skipped_spaces; }
|
||||
|
||||
/*! Affects the next call to all methods using get_token().
|
||||
The token is copied and stored within SrInput, leaving the user buffer untouched. */
|
||||
void unget_token ( const char *token, TokenType type );
|
||||
|
||||
/*! Affects the next call to all methods using get_token().
|
||||
The last token parsed is ungetted, if its type is not EndOfFile */
|
||||
void unget_token ();
|
||||
|
||||
/*! Returns true if any data (token or byte) was put in the unget stacks */
|
||||
bool has_unget_data () const;
|
||||
|
||||
/*! Puts a token in buf, returning its type, skipping comments, and considering
|
||||
unget() and unget_token() data. When EOF is reached, buf[0] is set to 0.
|
||||
Names are converted to lower case according to the state set by lowercase_tokens().
|
||||
The maximum length permitted for buf is determined by max_token_size().
|
||||
When SrInput::Error is returned, last_error() will return the description of
|
||||
the error occured.
|
||||
Note: get_token() will parse an input like "-3" as having two tokens, a
|
||||
delimiter and an integer (use getn() for reading numbers). */
|
||||
TokenType get_token ( SrString& buf );
|
||||
|
||||
/* Does the same as get_token(SrString&), but the parsed token and token type
|
||||
are kept in an internal buffer, to be later retrieved by last_token() and
|
||||
last_token_type() */
|
||||
TokenType get_token ();
|
||||
|
||||
/* Returns a reference to the internal buffer containing the last token parsed
|
||||
with get_token(void). */
|
||||
SrString& last_token() { return _last_token; }
|
||||
|
||||
/* Returns the type of the last token parsed with get_token(void). */
|
||||
TokenType last_token_type() const { return (TokenType)_last_token_type; }
|
||||
|
||||
/*! Returns the last error occured, if any. Errors are generated by methods
|
||||
using get_token() methods. */
|
||||
ErrorType last_error () const { return (ErrorType)_last_error; }
|
||||
|
||||
/*! Returns true if there an error occured, and false otherwise. */
|
||||
bool had_error () const { return ((ErrorType)_last_error)==NoError? false:true; }
|
||||
|
||||
/*! Sets the last error to NoError. */
|
||||
void reset_error () { _last_error=(srbyte)NoError; }
|
||||
|
||||
/*! This static method will return a describing error message if t is one
|
||||
of the following: UndefChar, UnexpectedToken, TooBig, OpenString, or InvalidPoint.
|
||||
Otherwise it will return 0 : */
|
||||
static char* error_desc ( ErrorType t );
|
||||
|
||||
/*! Gets the next token of type string or name using get_token(void),
|
||||
and returns the token, which is kept in the internal last_token() string.
|
||||
If the returned string has length 0 (ie equal to ""), it means that
|
||||
the parsed token is not a string, neithert a name, or EOF was reached.
|
||||
In any of these cases, last_error() will return UnexpectedToken. */
|
||||
SrString& gets ();
|
||||
|
||||
/*! Same as gets(void), but without using the internal buffer of last_token(),
|
||||
the given buffer buf is used instead. A reference to buf is retorned. */
|
||||
SrString& gets ( SrString& buf );
|
||||
|
||||
/*! Gets the next token, considering it is a number (type int or real)
|
||||
preceeded or not by delimiters + or -, and returns the token, which
|
||||
is kept in the internal last_token() string.
|
||||
The type in last_token_type() will tell if it was an integer or real.
|
||||
If the returned string has length 0 (ie equal to ""), it means that
|
||||
the parsed token is not a number or EOF was reached.
|
||||
In any of these cases, last_error() will return UnexpectedToken. */
|
||||
SrString& getn ();
|
||||
|
||||
/*! Reads the next token using get_token(), and tests if it is a delimiter.
|
||||
Returns the delimiter char if a delimiter was found, otherwise returns 0.
|
||||
When 0 is returned, last_error() will be set to UnexpectedToken. */
|
||||
char getd ();
|
||||
|
||||
/*! Skips next n tokens, by calling get_token(). n has a default value of 1.
|
||||
False is returned if any error occurred or EOF is encountered.
|
||||
Otherwise true is returned. */
|
||||
bool skip ( int n=1 );
|
||||
|
||||
/*! Reads tokens using get_token() until the given name is read as type Name.
|
||||
skipto() uses the sr_compare() function that is case-insensitive.
|
||||
Returns false if EOF was reached before, otherwise returns true. */
|
||||
bool skipto ( const char *name );
|
||||
|
||||
/*! Returns true if there is a field like <nnn> in the current position,
|
||||
or otherwise false is returned. All parsed tokens are ungetted, so that
|
||||
the effect is as the current input position is not changed. */
|
||||
bool has_field ();
|
||||
|
||||
/*! Parse the input expecting to encounter a field like <name>, and puts
|
||||
field_name in the string name. If unexpected tokens are read, an error occurs
|
||||
and false is returned. If EOF is encountered, false is returned. */
|
||||
bool read_field ( SrString& name );
|
||||
|
||||
/*! Checks if the following tokens in input is </name>, returning true or false. */
|
||||
bool close_field ( const SrString& name );
|
||||
|
||||
/*! Reads the input until reading </name>. If the field is not found, false is
|
||||
returned. Otherwise true is returned and the input will point after </name>. */
|
||||
bool skip_field ( const SrString& name );
|
||||
|
||||
/*! Operator to read an integer using method getn() and function atoi().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, int& i );
|
||||
|
||||
/*! Operator to read an unsigned integer using method getn() and function atoi().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, sruint& i );
|
||||
|
||||
/*! Operator to read an unsigned char using method getn() and function atoi().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, srbyte& c );
|
||||
|
||||
/*! Operator to read a float using method getn() and function atof().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, float& f );
|
||||
|
||||
/*! Operator to read a double using method getn() and function atof().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, double& d );
|
||||
|
||||
/*! Operator to read a string using method gets(). st must point to a buffer
|
||||
with enought space to receive the input. The input is read using the
|
||||
internal buffer, and the result is copied to st using simply strcpy().
|
||||
Errors can be tracked using last_error(). */
|
||||
friend SrInput& operator>> ( SrInput& in, char* st );
|
||||
|
||||
/*! Operator to read a SrString using method gets(SrString&).
|
||||
Errors can be tracked using last_error(). Implemented inline. */
|
||||
friend SrInput& operator>> ( SrInput& in, SrString& s ) { in.gets(s); return in; }
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_INPUT_H
|
||||
29
source/dcdt/se/sr_light.cpp
Normal file
29
source/dcdt/se/sr_light.cpp
Normal file
@ -0,0 +1,29 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_light.h"
|
||||
|
||||
//===================================== SrLight ================================
|
||||
|
||||
|
||||
SrLight::SrLight ()
|
||||
{
|
||||
init ();
|
||||
}
|
||||
|
||||
void SrLight::init ()
|
||||
{
|
||||
spot_exponent = 0;
|
||||
spot_direction.set ( 0, 0, -1.0f );
|
||||
spot_cutoff = 180;
|
||||
constant_attenuation = 1.0f;
|
||||
linear_attenuation = 0;
|
||||
quadratic_attenuation = 0;
|
||||
ambient = SrColor::black;
|
||||
diffuse = SrColor::white;
|
||||
specular = SrColor::black;
|
||||
position = SrVec::k;
|
||||
directional = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//================================ End of File =================================================
|
||||
82
source/dcdt/se/sr_light.h
Normal file
82
source/dcdt/se/sr_light.h
Normal file
@ -0,0 +1,82 @@
|
||||
|
||||
# ifndef SR_LIGHT_H
|
||||
# define SR_LIGHT_H
|
||||
|
||||
/** \file sr_light.h
|
||||
* Keeps light parameters
|
||||
*/
|
||||
|
||||
# include "sr_vec.h"
|
||||
# include "sr_color.h"
|
||||
|
||||
/*! \class SrLight sr_light.h
|
||||
\brief Keeps light parameters
|
||||
|
||||
SrLight Keeps light parameters following the OpenGL specifications.
|
||||
Note however that this class only stores data and has no dependency on
|
||||
OpenGL functions. */
|
||||
class SrLight
|
||||
{ public :
|
||||
|
||||
/*! Higher spot exponents result in a more focused light source. The default
|
||||
spot exponent is 0, resulting in uniform light distribution. Values must
|
||||
be in the range of [0,128]. */
|
||||
float spot_exponent;
|
||||
|
||||
/*! The direction of the light in homogeneous object coordinates. The spot
|
||||
direction is transformed by the inverse of the modelview matrix when
|
||||
glLight is called (just as if it was a normal), and it is stored in
|
||||
eye coordinates. It is significant only when GL_SPOT_CUTOFF is not
|
||||
180, which it is by default. The default direction is (0,0,-1). */
|
||||
SrVec spot_direction;
|
||||
|
||||
/*! Specifies the maximum spread angle. Only values in the range [0,90],
|
||||
and the special value 180, are accepted. If the angle between the
|
||||
direction of the light and the direction from the light to the vertex
|
||||
being lighted is greater than the spot cutoff angle, then the light
|
||||
is completely masked. Otherwise, its intensity is controlled by the
|
||||
spot exponent and the attenuation factors. The default spot cutoff
|
||||
is 180, resulting in uniform light distribution. */
|
||||
float spot_cutoff;
|
||||
|
||||
/*! If the light is positional, rather than directional, its intensity
|
||||
is attenuated by the reciprocal of the sum of: the constant factor,
|
||||
the linear factor multiplied by the distance between the light and
|
||||
the vertex being lighted, and the quadratic factor multiplied by
|
||||
the square of the same distance. The default attenuation factors
|
||||
are cte=1, linear=0, quad=0, resulting in no attenuation. Only
|
||||
nonnegative values are accepted. */
|
||||
float constant_attenuation;
|
||||
float linear_attenuation; //!< See constant_attenuation.
|
||||
float quadratic_attenuation; //!< See constant_attenuation.
|
||||
|
||||
SrColor ambient; //!< Default is black
|
||||
SrColor diffuse; //!< Default is white
|
||||
SrColor specular; //!< Default is black
|
||||
|
||||
/*! The position is transformed by the modelview matrix when glLight is
|
||||
called (just as if it was a point), and it is stored in eye
|
||||
coordinates. If directional, diffuse and specular lighting calculations
|
||||
take the lights direction, but not its actual position, into account,
|
||||
and attenuation is disabled. Otherwise, diffuse and specular lighting
|
||||
calculations are based on the actual location of the light in eye
|
||||
coordinates, and attenuation is enabled. The default position is (0,0,1). */
|
||||
SrVec position;
|
||||
|
||||
/*! When true means that the position w coord is 0, otherwise 1. Default is true. */
|
||||
bool directional;
|
||||
|
||||
public :
|
||||
|
||||
/*! Initialize the camera with the default parameters. */
|
||||
SrLight ();
|
||||
|
||||
/*! Sets the default parameters. */
|
||||
void init ();
|
||||
};
|
||||
|
||||
|
||||
//================================ End of File =================================================
|
||||
|
||||
# endif // SR_LIGHT_H
|
||||
|
||||
177
source/dcdt/se/sr_line.cpp
Normal file
177
source/dcdt/se/sr_line.cpp
Normal file
@ -0,0 +1,177 @@
|
||||
#include "precompiled.h"
|
||||
# include <math.h>
|
||||
# include "sr_box.h"
|
||||
# include "sr_line.h"
|
||||
# include "sr_input.h"
|
||||
# include "sr_output.h"
|
||||
|
||||
//===========================================================================
|
||||
|
||||
// Important: Static initializations cannot use other static initialized
|
||||
// variables ( as SrVec::i, ... ), as we cannot know the order that they
|
||||
// will be initialized by the compiler.
|
||||
|
||||
const SrLine SrLine::x ( SrVec(0,0,0), SrVec(1.0f,0,0) );
|
||||
const SrLine SrLine::y ( SrVec(0,0,0), SrVec(0,1.0f,0) );
|
||||
const SrLine SrLine::z ( SrVec(0,0,0), SrVec(0,0,1.0f) );
|
||||
|
||||
//============================== SrLine ====================================
|
||||
|
||||
#define EPSILON 0.00001 // floats have 7 decimals
|
||||
|
||||
// Original code from : http://www.acm.org/jgt/papers/MollerTrumbore97/
|
||||
bool SrLine::intersects_triangle ( const SrVec &v0, const SrVec &v1, const SrVec &v2,
|
||||
float &t, float &u, float &v ) const
|
||||
{
|
||||
SrVec dir, edge1, edge2, tvec, pvec, qvec;
|
||||
float det, inv_det;
|
||||
|
||||
dir = p2 - p1;
|
||||
edge1 = v1 - v0; // find vectors for two edges sharing v0
|
||||
edge2 = v2 - v0;
|
||||
pvec = cross ( dir, edge2 ); // begin calculating determinant - also used to calculate U parameter
|
||||
det = dot ( edge1, pvec ); // if determinant is near zero, ray lies in plane of triangle
|
||||
// printf("det=%f\n",det);
|
||||
|
||||
if ( SR_NEXTZ(det,EPSILON) )
|
||||
{ //sr_out.warning("det in ray_triangle fails => %f",(float)det);
|
||||
return false;
|
||||
}
|
||||
inv_det = 1.0f / det;
|
||||
|
||||
tvec = p1 - v0; // calculate distance from v0 to ray origin
|
||||
u = dot(tvec, pvec) * inv_det; // calculate U parameter and test bounds
|
||||
if ( u<0.0 || u>1.0 ) return false;
|
||||
|
||||
qvec = cross ( tvec, edge1 ); // prepare to test V parameter
|
||||
v = dot(dir, qvec) * inv_det; // calculate V parameter and test bounds
|
||||
if ( v<0.0 || u+v>1.0 ) return false;
|
||||
t = dot(edge2,qvec) * inv_det; // calculate t, ray intersects triangle
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SrLine::intersects_square ( const SrVec &v1, const SrVec &v2,
|
||||
const SrVec &v3, const SrVec &v4, float& t ) const
|
||||
{
|
||||
float u, v;
|
||||
if ( intersects_triangle ( v1, v2, v3, t, u, v ) ) return true;
|
||||
if ( intersects_triangle ( v1, v3, v4, t, u, v ) ) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
int SrLine::intersects_box ( const SrBox& box, float& t1, float& t2, SrVec* vp ) const
|
||||
{
|
||||
SrVec p1, p2, p3, p4, p;
|
||||
float t[6];
|
||||
int side[6];
|
||||
int tsize=0;
|
||||
|
||||
# define INTERSECT(s) if ( intersects_square(p1,p2,p3,p4,t[tsize]) ) { side[tsize]=s; tsize++; }
|
||||
|
||||
box.get_side ( p1, p2, p3, p4, 0 );
|
||||
INTERSECT(0);
|
||||
box.get_side ( p1, p2, p3, p4, 1 );
|
||||
INTERSECT(1);
|
||||
box.get_side ( p1, p2, p3, p4, 2 );
|
||||
INTERSECT(2);
|
||||
box.get_side ( p1, p2, p3, p4, 3 );
|
||||
INTERSECT(3);
|
||||
box.get_side ( p1, p2, p3, p4, 4 );
|
||||
INTERSECT(4);
|
||||
box.get_side ( p1, p2, p3, p4, 5 );
|
||||
INTERSECT(5);
|
||||
|
||||
# undef INTERSECT
|
||||
|
||||
if ( tsize==0 )
|
||||
{ t1=t2=0; }
|
||||
else if ( tsize==1 )
|
||||
{ t1=t2=t[0]; }
|
||||
else if ( tsize==2 )
|
||||
{ float tmpf;
|
||||
int tmpi;
|
||||
if ( t[1]<t[0] ) { SR_SWAPT(t[0],t[1],tmpf); SR_SWAPT(side[0],side[1],tmpi); }
|
||||
t1 = t[0];
|
||||
t2 = t[1];
|
||||
}
|
||||
else // sort according to t and take the two extremes
|
||||
{ int i, j, tmpi;
|
||||
float tmpf;
|
||||
for ( i=0; i<tsize; i++ )
|
||||
for ( j=i; j<tsize; j++ )
|
||||
if ( t[j]<t[i] ) { SR_SWAPT(t[i],t[j],tmpf); SR_SWAPT(side[i],side[j],tmpi); }
|
||||
t1 = t[0];
|
||||
t2 = t[tsize-1];
|
||||
tsize = 2;
|
||||
}
|
||||
|
||||
if (tsize>0 && vp) box.get_side ( vp[0], vp[1], vp[2], vp[3], side[0] );
|
||||
return tsize;
|
||||
}
|
||||
|
||||
int SrLine::intersects_sphere ( const SrPnt& center, float radius, SrPnt* vp ) const
|
||||
{
|
||||
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
|
||||
SrVec dir = p2-p1;
|
||||
SrVec kdiff = p1 - center;
|
||||
float a = dir.norm2();
|
||||
float b = dot ( kdiff, dir );
|
||||
float c = kdiff.norm2() - radius*radius;
|
||||
|
||||
float aft[2];
|
||||
float discr = b*b - a*c;
|
||||
|
||||
if ( discr < 0.0f )
|
||||
{ return 0;
|
||||
}
|
||||
else if ( discr > 0.0f )
|
||||
{ float root = sqrtf(discr);
|
||||
float inva = 1.0f/a;
|
||||
aft[0] = (-b - root)*inva;
|
||||
aft[1] = (-b + root)*inva;
|
||||
if ( vp )
|
||||
{ vp[0] = p1 + aft[0]*dir;
|
||||
vp[1] = p1 + aft[1]*dir;
|
||||
if ( dist2(vp[1],p1)<dist2(vp[0],p1) )
|
||||
{ SrPnt tmp;
|
||||
SR_SWAP(vp[0],vp[1]);
|
||||
}
|
||||
}
|
||||
return 2;
|
||||
}
|
||||
else
|
||||
{ aft[0] = -b/a;
|
||||
if ( vp ) vp[0] = p1 + aft[0]*dir;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
//http://astronomy.swin.edu.au/~pbourke/geometry/pointline/source.c
|
||||
SrPnt SrLine::closestpt ( SrPnt p, float* k ) const
|
||||
{
|
||||
SrVec v (p2-p1);
|
||||
|
||||
float u = ( ( (p.x-p1.x) * (v.x) ) +
|
||||
( (p.y-p1.y) * (v.y) ) +
|
||||
( (p.z-p1.z) * (v.z) ) ) / ( v.norm2() );
|
||||
|
||||
//if( u<0.0f || u>1.0f ) // closest point does not fall within the line segment
|
||||
if ( k ) *k=u;
|
||||
|
||||
return p1 + u*v;
|
||||
}
|
||||
|
||||
//============================== friends ====================================
|
||||
|
||||
SrOutput& operator<< ( SrOutput& o, const SrLine& l )
|
||||
{
|
||||
return o << l.p1 <<" "<< l.p2;
|
||||
}
|
||||
|
||||
SrInput& operator>> ( SrInput& in, SrLine& l )
|
||||
{
|
||||
return in >> l.p1 >> l.p2;
|
||||
}
|
||||
|
||||
//============================= End of File ===========================================
|
||||
95
source/dcdt/se/sr_line.h
Normal file
95
source/dcdt/se/sr_line.h
Normal file
@ -0,0 +1,95 @@
|
||||
/** \file sr_line.h
|
||||
* Three dimensional line */
|
||||
|
||||
# ifndef SR_LINE_H
|
||||
# define SR_LINE_H
|
||||
|
||||
# include "sr_vec.h"
|
||||
|
||||
class SrBox;
|
||||
class SrInput;
|
||||
class SrOutput;
|
||||
|
||||
/*! \class SrLine sr_line.h
|
||||
\brief Three dimensional line
|
||||
|
||||
SrLine defines a line by keeping two points in the three dimensional
|
||||
space. These two points are p1 and p2 of type SrVec(==SrPnt). When the line
|
||||
is considered as a segement, p1 and p2 will delimit the segment, and
|
||||
when the line is considered as a ray, the source of the ray is p1 and
|
||||
the ray direction is defined as p2-p1. */
|
||||
class SrLine
|
||||
{ public :
|
||||
SrPnt p1, p2;
|
||||
public :
|
||||
static const SrLine x; //!< (0,0,0)--(1,0,0) line
|
||||
static const SrLine y; //!< (0,0,0)--(0,1,0) line
|
||||
static const SrLine z; //!< (0,0,0)--(0,0,1) line
|
||||
public :
|
||||
|
||||
/*! Initializes SrLine as the x axis (0,0,0)--(1,0,0) line. */
|
||||
SrLine () : p1(SrPnt::null), p2(SrPnt::i) {}
|
||||
|
||||
/*! Copy constructor. */
|
||||
SrLine ( const SrLine &l ) : p1(l.p1), p2(l.p2) {}
|
||||
|
||||
/*! Initializes with the given endpoints. */
|
||||
SrLine ( const SrPnt &v1, const SrPnt &v2 ) : p1(v1), p2(v2) {}
|
||||
|
||||
/*! Set endpoints. */
|
||||
void set ( const SrPnt &v1, const SrPnt &v2 ) { p1=v1; p2=v2; }
|
||||
|
||||
/*! Same as copy operator. */
|
||||
void set ( const SrLine &l ) { p1=l.p1; p2=l.p2; }
|
||||
|
||||
/*! Copy operator from another SrLine. */
|
||||
void operator = ( const SrLine &l ) { p1=l.p1; p2=l.p2; }
|
||||
|
||||
/*! Calculates the intersection of SrLine with the triangle [v0,v1,v2].
|
||||
If the line intercepts the triangle, true is returned, otherwise
|
||||
false is returned. The triangle can be given in any orientation.
|
||||
When true is returned the return values t,u,v will satisfy:
|
||||
(1-u-v)v0 + uv1 + vv2 == (1-t)p1 + (t)p2 == interception point.
|
||||
In this way, u and v indicate a parametric distance from the vertices
|
||||
and t is a parametric distance that can be used to determine if only
|
||||
the segment [p1,p2] intersects in fact the triangle. */
|
||||
bool intersects_triangle ( const SrPnt &v0, const SrPnt &v1, const SrPnt &v2,
|
||||
float &t, float &u, float &v ) const;
|
||||
|
||||
/*! Returns the number of intersections between the line and the square (v1,v2,v3,v4).
|
||||
In case true is returned, the intersection point is
|
||||
defined by (1-t)p1 + (t)p2, so that if t is between 0 and 1, the point
|
||||
is also inside the segment determined by SrLine. */
|
||||
bool intersects_square ( const SrPnt& v1, const SrPnt& v2,
|
||||
const SrPnt& v3, const SrPnt& v4, float& t ) const;
|
||||
|
||||
/*! Returns 1 or 2 if the line intersects the box, otherwise 0 is returned.
|
||||
In case 2 is returned, there are two intersection points defined by
|
||||
(1-t1)p1 + (t1)p2, and (1-t2)p1 + (t2)p2 (t1<t2).
|
||||
In case 1 is returned, there is one intersection point (1-t1)p1 + (t1)p2,
|
||||
and t1 is equal to t2. Parameter vp is a pointer to SrPnt[4], and will contain
|
||||
the corners of the traversed side of the box, if vp is not null. */
|
||||
int intersects_box ( const SrBox& box, float& t1, float& t2, SrPnt* vp=0 ) const;
|
||||
|
||||
/*! Returns 1 or 2 if the line intersects the sphere (center,radius), otherwise
|
||||
0 is returned. In case 1 or 2 is returned, there are one or two intersection
|
||||
points, which are put in the vp array, if the vp pointer is not null.
|
||||
If two intersection points exist they are ordered according to the proximity
|
||||
to SrLine::p1 */
|
||||
int intersects_sphere ( const SrPnt& center, float radius, SrPnt* vp=0 ) const;
|
||||
|
||||
/*! Returns the closest point in the line to p. Parameter k, if given,
|
||||
will be such that: closestpt == p1+k*(p2-p1) */
|
||||
SrPnt closestpt ( SrPnt p, float* k=0 ) const;
|
||||
|
||||
/*! Outputs in format: "p1 p2". */
|
||||
friend SrOutput& operator<< ( SrOutput& o, const SrLine& l );
|
||||
|
||||
/*! Inputs from format: "p1 p2". */
|
||||
friend SrInput& operator>> ( SrInput& in, SrLine& l );
|
||||
|
||||
};
|
||||
|
||||
//============================== end of file ===============================
|
||||
|
||||
# endif // SR_LINE_H
|
||||
265
source/dcdt/se/sr_lines.cpp
Normal file
265
source/dcdt/se/sr_lines.cpp
Normal file
@ -0,0 +1,265 @@
|
||||
#include "precompiled.h"
|
||||
# include "sr_box.h"
|
||||
# include "sr_mat.h"
|
||||
# include "sr_vec2.h"
|
||||
# include "sr_lines.h"
|
||||
|
||||
//# define SR_USE_TRACE1 // Constructor and Destructor
|
||||
# include "sr_trace.h"
|
||||
|
||||
//======================================= SrLines ====================================
|
||||
|
||||
const char* SrLines::class_name = "Lines";
|
||||
|
||||
SrLines::SrLines ()
|
||||
{
|
||||
SR_TRACE1 ( "Constructor" );
|
||||
}
|
||||
|
||||
SrLines::~SrLines ()
|
||||
{
|
||||
SR_TRACE1 ( "Destructor" );
|
||||
}
|
||||
|
||||
void SrLines::init ()
|
||||
{
|
||||
V.size(0);
|
||||
C.size(0);
|
||||
I.size(0);
|
||||
}
|
||||
|
||||
void SrLines::compress ()
|
||||
{
|
||||
V.compress();
|
||||
C.compress();
|
||||
I.compress();
|
||||
}
|
||||
|
||||
void SrLines::push_line ( const SrVec &p1, const SrVec &p2 )
|
||||
{
|
||||
V.push()=p1;
|
||||
V.push()=p2;
|
||||
}
|
||||
|
||||
void SrLines::push_line ( const SrVec2 &p1, const SrVec2 &p2 )
|
||||
{
|
||||
V.push().set(p1.x,p1.y,0);
|
||||
V.push().set(p2.x,p2.y,0);
|
||||
}
|
||||
|
||||
void SrLines::push_line ( float ax, float ay, float az, float bx, float by, float bz )
|
||||
{
|
||||
V.push().set(ax,ay,az);
|
||||
V.push().set(bx,by,bz);
|
||||
}
|
||||
|
||||
void SrLines::push_line ( float ax, float ay, float bx, float by )
|
||||
{
|
||||
V.push().set(ax,ay,0);
|
||||
V.push().set(bx,by,0);
|
||||
}
|
||||
|
||||
void SrLines::begin_polyline ()
|
||||
{
|
||||
I.push() = V.size();
|
||||
}
|
||||
|
||||
void SrLines::end_polyline ()
|
||||
{
|
||||
I.push() = V.size()-1;
|
||||
}
|
||||
|
||||
void SrLines::push_vertex ( const SrVec& p )
|
||||
{
|
||||
V.push() = p;
|
||||
}
|
||||
|
||||
void SrLines::push_vertex ( const SrVec2& p )
|
||||
{
|
||||
V.push().set(p.x,p.y,0);
|
||||
}
|
||||
|
||||
void SrLines::push_vertex ( float x, float y, float z )
|
||||
{
|
||||
V.push().set(x,y,z);
|
||||
}
|
||||
|
||||
void SrLines::push_color ( const SrColor &c )
|
||||
{
|
||||
I.push() = V.size();
|
||||
C.push() = c;
|
||||
I.push() = -C.size();
|
||||
}
|
||||
|
||||
void SrLines::push_cross ( SrVec2 c, float r )
|
||||
{
|
||||
SrVec2 p(r,r);
|
||||
push_line ( c-p, c+p ); p.x*=-1;
|
||||
push_line ( c-p, c+p );
|
||||
}
|
||||
|
||||
void SrLines::push_axis ( const SrPnt& orig, float len, int dim, const char* let,
|
||||
bool rule, SrBox* box )
|
||||
{
|
||||
float r, mr;
|
||||
float a, b, c, k;
|
||||
const float z = 0.0f;
|
||||
|
||||
r = box? box->max_size()/2.0f : len;
|
||||
mr = -r;
|
||||
a=r/25.0f; b=a/2.0f; c=a*3.0f; k=a/3.0f;
|
||||
|
||||
bool letx=false, lety=false, letz=false;
|
||||
int vi = V.size();
|
||||
|
||||
if ( let )
|
||||
while ( *let )
|
||||
{ char c = SR_UPPER(*let);
|
||||
if ( c=='X' ) letx=true;
|
||||
else if ( c=='Y' ) lety=true;
|
||||
else if ( c=='Z' ) letz=true;
|
||||
let++;
|
||||
}
|
||||
|
||||
if ( dim>=1 )
|
||||
{ if ( box ) { mr=box->a.x; r=box->b.x; }
|
||||
push_color ( SrColor::red );
|
||||
push_line ( mr, z, z, r, z, z ); // X axis
|
||||
if ( letx && r>0 )
|
||||
{ push_line ( r, -a, z, r-a, -c, z ); // Letter X
|
||||
push_line ( r-a, -a, z, r, -c, z );
|
||||
}
|
||||
if ( rule && r>1.0 )
|
||||
{ int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r);
|
||||
push_color ( SrColor::red );
|
||||
for ( i=mini; i<maxi; i++ )
|
||||
{ if ( i==0 ) continue;
|
||||
push_line ( (float)i, z, z, (float)i, k, z );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( dim>=2 )
|
||||
{ if ( box ) { mr=box->a.y; r=box->b.y; }
|
||||
push_color ( SrColor::green );
|
||||
push_line ( z, mr, z, z, r, z ); // Y axis
|
||||
if ( lety && r>0 )
|
||||
{ push_line ( a, r, z, a+b, r-a, z ); // Letter Y
|
||||
push_line ( a+a, r, z, a, r-a-a, z );
|
||||
}
|
||||
if ( rule && r>1.0 )
|
||||
{ int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r);
|
||||
push_color ( SrColor::green );
|
||||
for ( i=mini; i<maxi; i++ )
|
||||
{ if ( i==0 ) continue;
|
||||
push_line ( z, (float)i, z, -k, (float)i, z );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( dim>=3 )
|
||||
{ if ( box ) { mr=box->a.z; r=box->b.z; }
|
||||
push_color ( SrColor::blue );
|
||||
push_line ( z, z, mr, z, z, r ); // Z axis
|
||||
if ( letz && r>0 )
|
||||
{ begin_polyline ();
|
||||
push_vertex ( z, -a, r-a ); // Letter Z
|
||||
push_vertex ( z, -a, r );
|
||||
push_vertex ( z, -c, r-a );
|
||||
push_vertex ( z, -c, r );
|
||||
end_polyline ();
|
||||
}
|
||||
if ( rule && r>1.0 )
|
||||
{ int i; int mini=SR_CEIL(mr); int maxi=SR_FLOOR(r);
|
||||
push_color ( SrColor::blue );
|
||||
for ( i=mini; i<maxi; i++ )
|
||||
{ if ( i==0 ) continue;
|
||||
push_line ( z, z, (float)i, z, k, (float)i );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ( orig!=SrPnt::null )
|
||||
{ int i;
|
||||
for ( i=vi; i<V.size(); i++ ) V[i]+=orig;
|
||||
}
|
||||
}
|
||||
|
||||
void SrLines::push_box ( const SrBox& box, bool multicolor )
|
||||
{
|
||||
const SrPnt& a = box.a;
|
||||
const SrPnt& b = box.b;
|
||||
|
||||
if ( multicolor ) push_color ( SrColor::red );
|
||||
push_line ( a.x, a.y, a.z, b.x, a.y, a.z );
|
||||
push_line ( a.x, a.y, b.z, b.x, a.y, b.z );
|
||||
push_line ( a.x, b.y, a.z, b.x, b.y, a.z );
|
||||
push_line ( a.x, b.y, b.z, b.x, b.y, b.z );
|
||||
|
||||
if ( multicolor ) push_color ( SrColor::green );
|
||||
push_line ( a.x, a.y, a.z, a.x, b.y, a.z );
|
||||
push_line ( a.x, a.y, b.z, a.x, b.y, b.z );
|
||||
push_line ( b.x, a.y, b.z, b.x, b.y, b.z );
|
||||
push_line ( b.x, a.y, a.z, b.x, b.y, a.z );
|
||||
|
||||
if ( multicolor ) push_color ( SrColor::blue );
|
||||
push_line ( a.x, a.y, a.z, a.x, a.y, b.z );
|
||||
push_line ( a.x, b.y, a.z, a.x, b.y, b.z );
|
||||
push_line ( b.x, b.y, a.z, b.x, b.y, b.z );
|
||||
push_line ( b.x, a.y, a.z, b.x, a.y, b.z );
|
||||
}
|
||||
|
||||
void SrLines::push_polyline ( const SrArray<SrVec2>& a )
|
||||
{
|
||||
int i;
|
||||
if ( a.size()<2 ) return;
|
||||
I.push() = V.size();
|
||||
for ( i=0; i<a.size(); i++ ) V.push().set(a[i].x,a[i].y,0);
|
||||
I.push() = V.size()-1;
|
||||
}
|
||||
|
||||
void SrLines::push_polygon ( const SrArray<SrVec2>& a )
|
||||
{
|
||||
int i;
|
||||
if ( a.size()<2 ) return;
|
||||
I.push() = V.size();
|
||||
for ( i=0; i<a.size(); i++ ) V.push().set(a[i].x,a[i].y,0);
|
||||
V.push().set(a[0].x,a[0].y,0);
|
||||
I.push() = V.size()-1;
|
||||
}
|
||||
|
||||
void SrLines::push_lines ( const SrArray<SrVec2>& a )
|
||||
{
|
||||
int i;
|
||||
for ( i=0; i<a.size(); i++ ) V.push().set(a[i].x,a[i].y,0);
|
||||
}
|
||||
|
||||
void SrLines::push_circle_approximation ( const SrPnt& center, const SrVec& radius,
|
||||
const SrVec& normal, int nvertices )
|
||||
{
|
||||
SrVec x = radius; // rotating vec to draw the circle
|
||||
SrVec x1st = x;
|
||||
|
||||
SrMat mr;
|
||||
float dr = sr2pi / (float)nvertices;
|
||||
mr.rot ( normal, dr );
|
||||
|
||||
begin_polyline ();
|
||||
while ( nvertices-->0 )
|
||||
{ push_vertex ( center + x );
|
||||
x = x*mr;
|
||||
}
|
||||
push_vertex ( center+x1st ); // to make it close exactly
|
||||
end_polyline ();
|
||||
}
|
||||
|
||||
void SrLines::get_bounding_box ( SrBox& b ) const
|
||||
{
|
||||
int i;
|
||||
b.set_empty ();
|
||||
for ( i=0; i<V.size(); i++ )
|
||||
{ b.extend ( V[i] );
|
||||
}
|
||||
}
|
||||
|
||||
//================================ EOF =================================================
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user