Point2 Exact_adaptive_kernel::circumcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double denominator = 0.5/(2.0*signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
return Point2(p1.x()+dx, p1.y()+dy);
}
static void drawVoronoiDual(std::vector<T*> &elements)
{
glColor4ubv((GLubyte *) & CTX::instance()->color.fg);
glEnable(GL_LINE_STIPPLE);
glLineStipple(1, 0x0F0F);
gl2psEnable(GL2PS_LINE_STIPPLE);
glBegin(GL_LINES);
for(unsigned int i = 0; i < elements.size(); i++){
T *ele = elements[i];
if(!isElementVisible(ele)) continue;
SPoint3 pc = ele->circumcenter();
if(ele->getDim() == 2){
for(int j = 0; j < ele->getNumEdges(); j++){
MEdge e = ele->getEdge(j);
SVector3 p2p1(e.getVertex(1)->x() - e.getVertex(0)->x(),
e.getVertex(1)->y() - e.getVertex(0)->y(),
e.getVertex(1)->z() - e.getVertex(0)->z());
SVector3 pcp1(pc.x() - e.getVertex(0)->x(),
pc.y() - e.getVertex(0)->y(),
pc.z() - e.getVertex(0)->z());
double alpha = dot(pcp1,p2p1) / dot(p2p1,p2p1);
SPoint3 p((1 - alpha)*e.getVertex(0)->x() + alpha * e.getVertex(1)->x(),
(1 - alpha)*e.getVertex(0)->y() + alpha * e.getVertex(1)->y(),
(1 - alpha)*e.getVertex(0)->z() + alpha * e.getVertex(1)->z());
glVertex3d(pc.x(), pc.y(), pc.z());
glVertex3d(p.x(), p.y(), p.z());
}
}
else if(ele->getDim() == 3){
for(int j = 0; j < ele->getNumFaces(); j++){
MFace f = ele->getFace(j);
SPoint3 p = f.barycenter();
glVertex3d(pc.x(), pc.y(), pc.z());
glVertex3d(p.x(), p.y(), p.z());
for(int k = 0; k < f.getNumVertices(); k++){
MEdge e(f.getVertex(k), (k == f.getNumVertices() - 1) ?
f.getVertex(0) : f.getVertex(k + 1));
SPoint3 pe = e.barycenter();
glVertex3d(p.x(), p.y(), p.z());
glVertex3d(pe.x(), pe.y(), pe.z());
}
}
}
}
glEnd();
glDisable(GL_LINE_STIPPLE);
gl2psDisable(GL2PS_LINE_STIPPLE);
}
Point2 Exact_adaptive_kernel::offcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3, double offconstant)
{
Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y());
Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y());
Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y());
double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y();
double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y();
double p2p3dist = p2p3.x()*p2p3.x() + p2p3.y()*p2p3.y();
double denominator = 0.5/(2.0*Exact_adaptive_kernel::signed_area(p1, p2, p3));
BOOST_ASSERT(denominator > 0.0);
double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator;
double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator;
double dxoff, dyoff;
if ((p2p1dist < p3p1dist) && (p2p1dist < p2p3dist)) {
dxoff = 0.5 * p2p1.x() - offconstant * p2p1.y();
dyoff = 0.5 * p2p1.y() + offconstant * p2p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else if (p3p1dist < p2p3dist) {
dxoff = 0.5 * p3p1.x() + offconstant * p3p1.y();
dyoff = 0.5 * p3p1.y() - offconstant * p3p1.x();
if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) {
dx = dxoff;
dy = dyoff;
}
} else {
dxoff = 0.5 * p2p3.x() - offconstant * p2p3.y();
dyoff = 0.5 * p2p3.y() + offconstant * p2p3.x();
if (dxoff * dxoff + dyoff * dyoff < (dx - p2p1.x()) * (dx - p2p1.x()) + (dy - p2p1.y()) * (dy - p2p1.y())) {
dx = p2p1.x() + dxoff;
dy = p2p1.y() + dyoff;
}
}
return Point2(p1.x()+dx, p1.y()+dy);
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * 2 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
// this is a degenerated hull hull to add some thickness and for a thick plane
delete convexHull;
dgStack<dgVector> tmp(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
tmp[i][0] = dgFloat32 (buffer[i*3 + 0]);
tmp[i][1] = dgFloat32 (buffer[i*3 + 1]);
tmp[i][2] = dgFloat32 (buffer[i*3 + 2]);
tmp[i][2] = dgFloat32 (0.0f);
}
dgObb sphere;
sphere.SetDimensions (&tmp[0][0], sizeof (dgVector), count);
dgInt32 index = 0;
dgFloat32 size = dgFloat32 (1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (sphere.m_size[i] < size) {
index = i;
size = sphere.m_size[i];
}
}
dgVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
normal[index] = dgFloat32 (1.0f);
dgVector step = sphere.RotateVector (normal.Scale3 (dgFloat32 (0.05f)));
for (dgInt32 i = 0; i < count; i ++) {
dgVector p1 (tmp[i] + step);
dgVector p2 (tmp[i] - step);
buffer[i * 3 + 0] = p1.m_x;
buffer[i * 3 + 1] = p1.m_y;
buffer[i * 3 + 2] = p1.m_z;
buffer[(i + count) * 3 + 0] = p2.m_x;
buffer[(i + count) * 3 + 1] = p2.m_y;
buffer[(i + count) * 3 + 2] = p2.m_z;
}
count *= 2;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
dgAssert (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgAssert (convexHull);
dgInt32 vertexCount = convexHull->GetVertexCount();
if (vertexCount < 4) {
delete convexHull;
return false;
}
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
// bool edgeRemoved = false;
// while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
// edgeRemoved = true;
// }
// if (edgeRemoved) {
// if (!CheckConvex (polyhedra, hullVertexArray)) {
// delete convexHull;
// return false;
// }
// }
while (RemoveCoplanarEdge (polyhedra, hullVertexArray));
}
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
if (vertexMap[edge->m_incidentVertex] == -1) {
vertexMap[edge->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_vertexToEdgeMapping = (const dgConvexSimplexEdge**) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgConvexSimplexEdge*)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (0.0f);
}
}
delete convexHull;
vertexCount = m_vertexCount;
mark = polyhedra.IncLRU();;
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &iter.GetNode()->GetInfo();
if (edge->m_mark != mark) {
dgEdge *ptr = edge;
do {
ptr->m_mark = mark;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge) ;
}
}
m_faceCount = 0;
dgStack<char> faceMarks (m_edgeCount);
memset (&faceMarks[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!faceMarks[i]) {
dgConvexSimplexEdge* ptr = face;
do {
dgAssert ((ptr - m_simplex) >= 0);
faceMarks[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
if (vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
// create a face structure for support vertex
dgStack<dgConvexBox> boxTree (vertexCount);
dgTree<dgVector,dgInt32> sortTree(GetAllocator());
dgStack<dgTree<dgVector,dgInt32>::dgTreeNode*> vertexNodeList(vertexCount);
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < vertexCount; i ++) {
const dgVector& p = m_vertex[i];
vertexNodeList[i] = sortTree.Insert (p, i);
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
boxTree[0].m_box[0] = minP;
boxTree[0].m_box[1] = maxP;
boxTree[0].m_leftBox = -1;
boxTree[0].m_rightBox = -1;
boxTree[0].m_vertexStart = 0;
boxTree[0].m_vertexCount = vertexCount;
dgInt32 boxCount = 1;
dgInt32 stack = 1;
dgInt32 stackBoxPool[64];
stackBoxPool[0] = 0;
while (stack) {
stack --;
dgInt32 boxIndex = stackBoxPool[stack];
dgConvexBox& box = boxTree[boxIndex];
if (box.m_vertexCount > DG_CONVEX_VERTEX_CHUNK_SIZE) {
dgVector median (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector varian (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < box.m_vertexCount; i ++) {
dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
median += p;
varian += p.CompProduct3 (p);
}
varian = varian.Scale3 (dgFloat32 (box.m_vertexCount)) - median.CompProduct3(median);
dgInt32 index = 0;
dgFloat64 maxVarian = dgFloat64 (-1.0e10f);
for (dgInt32 i = 0; i < 3; i ++) {
if (varian[i] > maxVarian) {
index = i;
maxVarian = varian[i];
}
}
dgVector center = median.Scale3 (dgFloat32 (1.0f) / dgFloat32 (box.m_vertexCount));
dgFloat32 test = center[index];
dgInt32 i0 = 0;
dgInt32 i1 = box.m_vertexCount - 1;
do {
for (; i0 <= i1; i0 ++) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i0]->GetInfo()[index];
if (val > test) {
break;
}
}
for (; i1 >= i0; i1 --) {
dgFloat32 val = vertexNodeList[box.m_vertexStart + i1]->GetInfo()[index];
if (val < test) {
break;
}
}
if (i0 < i1) {
dgSwap(vertexNodeList[box.m_vertexStart + i0], vertexNodeList[box.m_vertexStart + i1]);
i0++;
i1--;
}
} while (i0 <= i1);
if (i0 == 0){
i0 = box.m_vertexCount / 2;
}
if (i0 >= (box.m_vertexCount - 1)){
i0 = box.m_vertexCount / 2;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = i0; i < box.m_vertexCount; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_rightBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart + i0;
boxTree[boxCount].m_vertexCount = box.m_vertexCount - i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
{
dgVector minP ( dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (1.0e15f), dgFloat32 (0.0f));
dgVector maxP (-dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), -dgFloat32 (1.0e15f), dgFloat32 (0.0f));
for (dgInt32 i = 0; i < i0; i ++) {
const dgVector& p = vertexNodeList[box.m_vertexStart + i]->GetInfo();
minP.m_x = dgMin (p.m_x, minP.m_x);
minP.m_y = dgMin (p.m_y, minP.m_y);
minP.m_z = dgMin (p.m_z, minP.m_z);
maxP.m_x = dgMax (p.m_x, maxP.m_x);
maxP.m_y = dgMax (p.m_y, maxP.m_y);
maxP.m_z = dgMax (p.m_z, maxP.m_z);
}
box.m_leftBox = boxCount;
boxTree[boxCount].m_box[0] = minP;
boxTree[boxCount].m_box[1] = maxP;
boxTree[boxCount].m_leftBox = -1;
boxTree[boxCount].m_rightBox = -1;
boxTree[boxCount].m_vertexStart = box.m_vertexStart;
boxTree[boxCount].m_vertexCount = i0;
stackBoxPool[stack] = boxCount;
stack ++;
boxCount ++;
}
}
}
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
m_vertex[i] = vertexNodeList[i]->GetInfo();
vertexNodeList[i]->GetInfo().m_w = dgFloat32 (i);
}
m_supportTreeCount = boxCount;
m_supportTree = (dgConvexBox*) m_allocator->Malloc(dgInt32 (boxCount * sizeof(dgConvexBox)));
memcpy (m_supportTree, &boxTree[0], boxCount * sizeof(dgConvexBox));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const ptr = &m_simplex[i];
dgTree<dgVector,dgInt32>::dgTreeNode* const node = sortTree.Find(ptr->m_vertex);
dgInt32 index = dgInt32 (node->GetInfo().m_w);
ptr->m_vertex = dgInt16 (index);
}
}
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const edge = &m_simplex[i];
m_vertexToEdgeMapping[edge->m_vertex] = edge;
}
SetVolumeAndCG ();
return true;
}
bool dgCollisionConvexHull::Create (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const vertexArray, dgFloat32 tolerance)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgStack<dgFloat64> buffer(3 * count);
for (dgInt32 i = 0; i < count; i ++) {
buffer[i * 3 + 0] = vertexArray[i * stride + 0];
buffer[i * 3 + 1] = vertexArray[i * stride + 1];
buffer[i * 3 + 2] = vertexArray[i * stride + 2];
}
dgConvexHull3d* convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
if (!convexHull->GetCount()) {
delete convexHull;
return false;
}
// check for degenerated faces
for (bool success = false; !success; ) {
success = true;
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgStack<dgInt8> mask(convexHull->GetVertexCount());
memset (&mask[0], 1, mask.GetSizeInBytes());
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
const dgBigVector& p0 = hullVertexArray[face.m_index[0]];
const dgBigVector& p1 = hullVertexArray[face.m_index[1]];
const dgBigVector& p2 = hullVertexArray[face.m_index[2]];
dgBigVector p1p0 (p1 - p0);
dgBigVector p2p0 (p2 - p0);
dgBigVector normal (p2p0 * p1p0);
dgFloat64 mag2 = normal % normal;
if (mag2 < dgFloat64 (1.0e-6f * 1.0e-6f)) {
success = false;
dgInt32 index = -1;
dgBigVector p2p1 (p2 - p1);
dgFloat64 dist10 = p1p0 % p1p0;
dgFloat64 dist20 = p2p0 % p2p0;
dgFloat64 dist21 = p2p1 % p2p1;
if ((dist10 >= dist20) && (dist10 >= dist21)) {
index = 2;
} else if ((dist20 >= dist10) && (dist20 >= dist21)) {
index = 1;
} else if ((dist21 >= dist10) && (dist21 >= dist20)) {
index = 0;
}
_ASSERTE (index != -1);
mask[face.m_index[index]] = 0;
}
}
if (!success) {
dgInt32 count = 0;
dgInt32 vertexCount = convexHull->GetVertexCount();
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (mask[i]) {
buffer[count * 3 + 0] = hullVertexArray[i].m_x;
buffer[count * 3 + 1] = hullVertexArray[i].m_y;
buffer[count * 3 + 2] = hullVertexArray[i].m_z;
count ++;
}
}
delete convexHull;
convexHull = new (GetAllocator()) dgConvexHull3d (GetAllocator(), &buffer[0], 3 * sizeof (dgFloat64), count, tolerance);
}
}
dgInt32 vertexCount = convexHull->GetVertexCount();
const dgBigVector* const hullVertexArray = convexHull->GetVertexPool();
dgPolyhedra polyhedra (GetAllocator());
polyhedra.BeginFace();
for (dgConvexHull3d::dgListNode* node = convexHull->GetFirst(); node; node = node->GetNext()) {
dgConvexHull3DFace& face = node->GetInfo();
polyhedra.AddFace (face.m_index[0], face.m_index[1], face.m_index[2]);
}
polyhedra.EndFace();
if (vertexCount > 4) {
bool edgeRemoved = false;
while (RemoveCoplanarEdge (polyhedra, hullVertexArray)) {
edgeRemoved = true;
}
if (edgeRemoved) {
if (!CheckConvex (polyhedra, hullVertexArray)) {
return false;
}
}
}
dgInt32 maxEdgeCount = polyhedra.GetCount();
dgStack<dgEdge*> stack(1024 + maxEdgeCount);
dgEdge* firstFace = &polyhedra.GetRoot()->GetInfo();
_ASSERTE (firstFace->m_twin->m_next != firstFace);
dgInt32 stackIndex = 1;
stack[0] = firstFace;
dgStack<dgInt32> vertexMap(vertexCount);
memset (&vertexMap[0], -1, vertexCount * sizeof (dgInt32));
// m_edgeCount = 0;
// m_vertexCount = 0;
dgInt32 i1 = polyhedra.IncLRU();
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
if (vertexMap[edge0->m_incidentVertex] == -1) {
vertexMap[edge0->m_incidentVertex] = m_vertexCount;
m_vertexCount ++;
}
dgEdge* ptr = edge0;
do {
stack[stackIndex] = ptr->m_twin;
stackIndex++;
ptr->m_mark = i1;
ptr->m_userData = m_edgeCount;
m_edgeCount ++;
ptr = ptr->m_twin->m_next;
} while (ptr != edge0) ;
}
}
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
for (dgInt32 i = 0; i < vertexCount; i ++) {
if (vertexMap[i] != -1) {
m_vertex[vertexMap[i]] = hullVertexArray[i];
m_vertex[vertexMap[i]].m_w = dgFloat32 (1.0f);
}
}
i1 = polyhedra.IncLRU();
stackIndex = 1;
stack[0] = firstFace;
while (stackIndex) {
stackIndex --;
dgEdge* const edge0 = stack[stackIndex];
if (edge0->m_mark != i1) {
dgEdge *ptr = edge0;
do {
ptr->m_mark = i1;
stack[stackIndex] = ptr->m_twin;
stackIndex++;
dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
simplexPtr->m_vertex = vertexMap[ptr->m_incidentVertex];
simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
ptr = ptr->m_twin->m_next;
} while (ptr != edge0) ;
}
}
SetVolumeAndCG ();
m_faceCount = 0;
dgStack<char> mark (m_edgeCount);
memset (&mark[0], 0, m_edgeCount * sizeof (dgInt8));
dgStack<dgConvexSimplexEdge*> faceArray (m_edgeCount);
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgConvexSimplexEdge* const face = &m_simplex[i];
if (!mark[i]) {
dgConvexSimplexEdge* ptr = face;
do {
_ASSERTE ((ptr - m_simplex) >= 0);
mark[dgInt32 (ptr - m_simplex)] = '1';
ptr = ptr->m_next;
} while (ptr != face);
faceArray[m_faceCount] = face;
m_faceCount ++;
}
}
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
memcpy (m_faceArray, &faceArray[0], m_faceCount * sizeof(dgConvexSimplexEdge *));
delete convexHull;
return true;
}
Real
MaterialTensorAux::getTensorQuantity(const SymmTensor & tensor,
const MTA_ENUM quantity,
const MooseEnum & quantity_moose_enum,
const int index,
const Point * curr_point,
const Point * p1,
const Point * p2)
{
Real value(0);
if (quantity == MTA_COMPONENT)
{
value = tensor.component(index);
}
else if ( quantity == MTA_VONMISES )
{
value = std::sqrt(0.5*(
std::pow(tensor.xx() - tensor.yy(), 2) +
std::pow(tensor.yy() - tensor.zz(), 2) +
std::pow(tensor.zz() - tensor.xx(), 2) + 6 * (
std::pow(tensor.xy(), 2) +
std::pow(tensor.yz(), 2) +
std::pow(tensor.zx(), 2))));
}
else if ( quantity == MTA_PLASTICSTRAINMAG )
{
value = std::sqrt(2.0/3.0 * tensor.doubleContraction(tensor));
}
else if ( quantity == MTA_HYDROSTATIC )
{
value = tensor.trace()/3.0;
}
else if ( quantity == MTA_HOOP )
{
// This is the location of the stress. A vector from the cylindrical axis to this point will define the x' axis.
Point p0( *curr_point );
// The vector p1 + t*(p2-p1) defines the cylindrical axis. The point along this
// axis closest to p0 is found by the following for t:
const Point p2p1( *p2 - *p1 );
const Point p2p0( *p2 - p0 );
const Point p1p0( *p1 - p0 );
const Real t( -(p1p0*p2p1)/p2p1.size_sq() );
// The nearest point on the cylindrical axis to p0 is p.
const Point p( *p1 + t * p2p1 );
Point xp( p0 - p );
xp /= xp.size();
Point yp( p2p1/p2p1.size() );
Point zp( xp.cross( yp ));
//
// The following works but does more than we need
//
// // Rotation matrix R
// ColumnMajorMatrix R(3,3);
// // Fill with direction cosines
// R(0,0) = xp(0);
// R(1,0) = xp(1);
// R(2,0) = xp(2);
// R(0,1) = yp(0);
// R(1,1) = yp(1);
// R(2,1) = yp(2);
// R(0,2) = zp(0);
// R(1,2) = zp(1);
// R(2,2) = zp(2);
// // Rotate
// ColumnMajorMatrix tensor( _tensor[_qp].columnMajorMatrix() );
// ColumnMajorMatrix tensorp( R.transpose() * ( tensor * R ));
// // Hoop stress is the zz stress
// value = tensorp(2,2);
//
// Instead, tensorp(2,2) = R^T(2,:)*tensor*R(:,2)
//
const Real zp0( zp(0) );
const Real zp1( zp(1) );
const Real zp2( zp(2) );
value = (zp0*tensor(0,0)+zp1*tensor(1,0)+zp2*tensor(2,0))*zp0 +
(zp0*tensor(0,1)+zp1*tensor(1,1)+zp2*tensor(2,1))*zp1 +
(zp0*tensor(0,2)+zp1*tensor(1,2)+zp2*tensor(2,2))*zp2;
}
else if ( quantity == MTA_RADIAL )
{
// This is the location of the stress. A vector from the cylindrical axis to this point will define the x' axis
// which is the radial direction in which we want the stress.
Point p0( *curr_point );
// The vector p1 + t*(p2-p1) defines the cylindrical axis. The point along this
// axis closest to p0 is found by the following for t:
const Point p2p1( *p2 - *p1 );
const Point p2p0( *p2 - p0 );
const Point p1p0( *p1 - p0 );
const Real t( -(p1p0*p2p1)/p2p1.size_sq() );
// The nearest point on the cylindrical axis to p0 is p.
const Point p( *p1 + t * p2p1 );
Point xp( p0 - p );
xp /= xp.size();
const Real xp0( xp(0) );
const Real xp1( xp(1) );
const Real xp2( xp(2) );
value = (xp0*tensor(0,0)+xp1*tensor(1,0)+xp2*tensor(2,0))*xp0 +
(xp0*tensor(0,1)+xp1*tensor(1,1)+xp2*tensor(2,1))*xp1 +
(xp0*tensor(0,2)+xp1*tensor(1,2)+xp2*tensor(2,2))*xp2;
}
else if ( quantity == MTA_AXIAL )
{
// The vector p2p1=(p2-p1) defines the axis, which is the direction in which we want the stress.
Point p2p1( *p2 - *p1 );
p2p1 /= p2p1.size();
const Real axis0( p2p1(0) );
const Real axis1( p2p1(1) );
const Real axis2( p2p1(2) );
value = (axis0*tensor(0,0)+axis1*tensor(1,0)+axis2*tensor(2,0))*axis0 +
(axis0*tensor(0,1)+axis1*tensor(1,1)+axis2*tensor(2,1))*axis1 +
(axis0*tensor(0,2)+axis1*tensor(1,2)+axis2*tensor(2,2))*axis2;
}
else if ( quantity == MTA_MAXPRINCIPAL )
{
value = principalValue( tensor, 0 );
}
else if ( quantity == MTA_MEDPRINCIPAL )
{
value = principalValue( tensor, 1 );
}
else if ( quantity == MTA_MINPRINCIPAL )
{
value = principalValue( tensor, 2 );
}
else if ( quantity == MTA_FIRSTINVARIANT )
{
value = tensor.trace();
}
else if ( quantity == MTA_SECONDINVARIANT )
{
value =
tensor.xx()*tensor.yy() +
tensor.yy()*tensor.zz() +
tensor.zz()*tensor.xx() -
tensor.xy()*tensor.xy() -
tensor.yz()*tensor.yz() -
tensor.zx()*tensor.zx();
}
else if ( quantity == MTA_THIRDINVARIANT )
{
value =
tensor.xx()*tensor.yy()*tensor.zz() -
tensor.xx()*tensor.yz()*tensor.yz() +
tensor.xy()*tensor.zx()*tensor.yz() -
tensor.xy()*tensor.xy()*tensor.zz() +
tensor.zx()*tensor.xy()*tensor.yz() -
tensor.zx()*tensor.zx()*tensor.yy();
}
else if ( quantity == MTA_TRIAXIALITY )
{
Real hydrostatic = tensor.trace()/3.0;
Real von_mises = std::sqrt(0.5*(
std::pow(tensor.xx() - tensor.yy(), 2) +
std::pow(tensor.yy() - tensor.zz(), 2) +
std::pow(tensor.zz() - tensor.xx(), 2) + 6 * (
std::pow(tensor.xy(), 2) +
std::pow(tensor.yz(), 2) +
std::pow(tensor.zx(), 2))));
value = std::abs(hydrostatic / von_mises);
}
else if ( quantity == MTA_VOLUMETRICSTRAIN )
{
value =
tensor.trace() +
tensor.xx()*tensor.yy() +
tensor.yy()*tensor.zz() +
tensor.zz()*tensor.xx() +
tensor.xx()*tensor.yy()*tensor.zz();
}
else
{
mooseError("Unknown quantity in MaterialTensorAux: " + quantity_moose_enum.operator std::string());
}
return value;
}
