dgFloat32 dgBody::RayCast (const dgLineBox& line, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 maxT) const
{
dgAssert (filter);
dgVector l0 (line.m_l0);
dgVector l1 (line.m_l0 + (line.m_l1 - line.m_l0).Scale4 (dgMin(maxT, dgFloat32 (1.0f))));
if (dgRayBoxClip (l0, l1, m_minAABB, m_maxAABB)) {
// if (1) {
//l0 = dgVector (-20.3125000f, 3.54991579f, 34.3441200f, 0.0f);
//l1 = dgVector (-19.6875000f, 3.54257250f, 35.2211456f, 0.0f);
dgContactPoint contactOut;
const dgMatrix& globalMatrix = m_collision->GetGlobalMatrix();
dgVector localP0 (globalMatrix.UntransformVector (l0));
dgVector localP1 (globalMatrix.UntransformVector (l1));
dgVector p1p0 (localP1 - localP0);
if ((p1p0 % p1p0) > dgFloat32 (1.0e-12f)) {
dgFloat32 t = m_collision->RayCast (localP0, localP1, dgFloat32 (1.0f), contactOut, preFilter, this, userData);
if (t < dgFloat32 (1.0f)) {
dgVector p (globalMatrix.TransformVector(localP0 + (localP1 - localP0).Scale3(t)));
dgVector l1l0 (line.m_l1 - line.m_l0);
t = ((p - line.m_l0) % l1l0) / (l1l0 % l1l0);
if (t < maxT) {
dgAssert (t >= dgFloat32 (0.0f));
dgAssert (t <= dgFloat32 (1.0f));
contactOut.m_normal = globalMatrix.RotateVector (contactOut.m_normal);
maxT = filter (this, contactOut.m_collision0, p, contactOut.m_normal, contactOut.m_shapeId0, userData, t);
}
}
}
}
return maxT;
}
void Path::addArcTo(const FloatPoint& p1, const FloatPoint& p2, float radius)
{
FloatPoint p0(m_path.currentPosition());
FloatPoint p1p0((p0.x() - p1.x()), (p0.y() - p1.y()));
FloatPoint p1p2((p2.x() - p1.x()), (p2.y() - p1.y()));
float p1p0_length = sqrtf(p1p0.x() * p1p0.x() + p1p0.y() * p1p0.y());
float p1p2_length = sqrtf(p1p2.x() * p1p2.x() + p1p2.y() * p1p2.y());
double cos_phi = (p1p0.x() * p1p2.x() + p1p0.y() * p1p2.y()) / (p1p0_length * p1p2_length);
// The points p0, p1, and p2 are on the same straight line (HTML5, 4.8.11.1.8)
// We could have used areCollinear() here, but since we're reusing
// the variables computed above later on we keep this logic.
if (qFuzzyCompare(qAbs(cos_phi), 1.0)) {
m_path.lineTo(p1);
return;
}
float tangent = radius / tan(acos(cos_phi) / 2);
float factor_p1p0 = tangent / p1p0_length;
FloatPoint t_p1p0((p1.x() + factor_p1p0 * p1p0.x()), (p1.y() + factor_p1p0 * p1p0.y()));
FloatPoint orth_p1p0(p1p0.y(), -p1p0.x());
float orth_p1p0_length = sqrt(orth_p1p0.x() * orth_p1p0.x() + orth_p1p0.y() * orth_p1p0.y());
float factor_ra = radius / orth_p1p0_length;
// angle between orth_p1p0 and p1p2 to get the right vector orthographic to p1p0
double cos_alpha = (orth_p1p0.x() * p1p2.x() + orth_p1p0.y() * p1p2.y()) / (orth_p1p0_length * p1p2_length);
if (cos_alpha < 0.f)
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
FloatPoint p((t_p1p0.x() + factor_ra * orth_p1p0.x()), (t_p1p0.y() + factor_ra * orth_p1p0.y()));
// calculate angles for addArc
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
float sa = acos(orth_p1p0.x() / orth_p1p0_length);
if (orth_p1p0.y() < 0.f)
sa = 2 * piDouble - sa;
// anticlockwise logic
bool anticlockwise = false;
float factor_p1p2 = tangent / p1p2_length;
FloatPoint t_p1p2((p1.x() + factor_p1p2 * p1p2.x()), (p1.y() + factor_p1p2 * p1p2.y()));
FloatPoint orth_p1p2((t_p1p2.x() - p.x()), (t_p1p2.y() - p.y()));
float orth_p1p2_length = sqrtf(orth_p1p2.x() * orth_p1p2.x() + orth_p1p2.y() * orth_p1p2.y());
float ea = acos(orth_p1p2.x() / orth_p1p2_length);
if (orth_p1p2.y() < 0)
ea = 2 * piDouble - ea;
if ((sa > ea) && ((sa - ea) < piDouble))
anticlockwise = true;
if ((sa < ea) && ((ea - sa) > piDouble))
anticlockwise = true;
m_path.lineTo(t_p1p0);
addArc(p, radius, sa, ea, anticlockwise);
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
//hacd::HaF64 val0 = t0.GetAproximateValue();
//if (val0 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64(t0) < hacd::HaF64(0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
// hacd::HaF64 val1 = t1.GetAproximateValue();
// if (val1 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64 (t1) < hacd::HaF64 (0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
//hacd::HaF64 val2 = t2.GetAproximateValue();
//if (val2 < hacd::HaF64 (0.0f)) {
if (hacd::HaF64 (t2) < hacd::HaF64 (0.0f)) {
return dgBigVector (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
//hacd::HaF64 den = sum.GetAproximateValue();
#ifdef _DEBUG
//dgBigVector testpoint (A.Scale (val0 / den) + B.Scale (val1 / den) + C.Scale(val2 / den));
dgBigVector testpoint (A.Scale (t0 / sum) + B.Scale (t1 / sum) + C.Scale(t2 / sum));
hacd::HaF64 volume = ((B - A) * (C - A)) % (testpoint - A);
HACD_ASSERT (fabs (volume) < hacd::HaF64 (1.0e-12f));
#endif
// return dgBigVector (val0 / den, val1 / den, val2 / den, hacd::HaF32 (0.0f));
return dgBigVector (t0 / sum, t1 / sum, t2 / sum, hacd::HaF32 (0.0f));
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
dgFloat64 val0 = t0.GetAproximateValue();
if (val0 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
dgFloat64 val1 = t1.GetAproximateValue();
if (val1 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
dgFloat64 val2 = t2.GetAproximateValue();
if (val2 < dgFloat64 (0.0f)) {
return dgBigVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
dgFloat64 den = sum.GetAproximateValue();
#ifdef _DEBUG
dgBigVector testpoint (A.Scale (val0 / den) + B.Scale (val1 / den) + C.Scale(val2 / den));
dgFloat64 volume = ((B - A) * (C - A)) % (testpoint - A);
_ASSERTE (fabs (volume) < dgFloat64 (1.0e-12f));
#endif
return dgBigVector (val0 / den, val1 / den, val2 / den, dgFloat32 (0.0f));
}
void dNewtonCollision::DebugRenderCallback(void* userData, int vertexCount, const dFloat* faceVertec, int id)
{
DebugCallBack* const callbackInfo = (DebugCallBack*)userData;
dVector normal = dVector(0.0f);
dVector p0 (faceVertec[0], faceVertec[1], faceVertec[2], 0.0f);
dVector p1 (faceVertec[3], faceVertec[4], faceVertec[5], 0.0f);
dVector p1p0(p1 - p0);
for (int i = 2; i < vertexCount; i++) {
dVector p2(faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2], 0.0f);
dVector p2p0(p2 - p0);
normal += p1p0.CrossProduct(p2p0);
p1p0 = p2p0;
}
dFloat side = normal.DotProduct3 (callbackInfo->m_eyePoint - p0);
if (side > 0.0f) {
callbackInfo->m_callback(faceVertec, vertexCount);
}
}
void dgConvexHull4dTetraherum::Init (const dgHullVector* const points, dgInt32 v0, dgInt32 v1, dgInt32 v2, dgInt32 v3)
{
//{0, 1, 2, 3},
//{3, 0, 2, 1},
//{3, 2, 1, 0},
//{3, 1, 0, 2}
m_faces[0].m_index[0] = v0;
m_faces[0].m_index[1] = v1;
m_faces[0].m_index[2] = v2;
m_faces[0].m_index[3] = v3;
m_faces[1].m_index[0] = v3;
m_faces[1].m_index[1] = v0;
m_faces[1].m_index[2] = v2;
m_faces[1].m_index[3] = v1;
m_faces[2].m_index[0] = v3;
m_faces[2].m_index[1] = v2;
m_faces[2].m_index[2] = v1;
m_faces[2].m_index[3] = v0;
m_faces[3].m_index[0] = v3;
m_faces[3].m_index[1] = v1;
m_faces[3].m_index[2] = v0;
m_faces[3].m_index[3] = v2;
SetMark (0);
for (dgInt32 i = 0; i < 4; i ++) {
m_faces[i].m_twin = NULL;
}
#ifdef _DEBUG
dgBigVector p1p0 (points[v1] - points[v0]);
dgBigVector p2p0 (points[v2] - points[v0]);
dgBigVector p3p0 (points[v3] - points[v0]);
dgBigVector normal (p1p0.CrossProduct4(p2p0, p3p0));
dgFloat64 volume = normal.DotProduct4(normal).m_x;
dgAssert (volume > dgFloat64 (0.0f));
#endif
}
dgBigVector LineTriangleIntersection (const dgBigVector& p0, const dgBigVector& p1, const dgBigVector& A, const dgBigVector& B, const dgBigVector& C)
{
dgHugeVector ph0 (p0);
dgHugeVector ph1 (p1);
dgHugeVector Ah (A);
dgHugeVector Bh (B);
dgHugeVector Ch (C);
dgHugeVector p1p0 (ph1 - ph0);
dgHugeVector Ap0 (Ah - ph0);
dgHugeVector Bp0 (Bh - ph0);
dgHugeVector Cp0 (Ch - ph0);
dgGoogol t0 ((Bp0 * Cp0) % p1p0);
double val0 = t0.GetAproximateValue();
if (val0 < double (0.0f)) {
return dgBigVector (float (0.0f), float (0.0f), float (0.0f), float (-1.0f));
}
dgGoogol t1 ((Cp0 * Ap0) % p1p0);
double val1 = t1.GetAproximateValue();
if (val1 < double (0.0f)) {
return dgBigVector (float (0.0f), float (0.0f), float (0.0f), float (-1.0f));
}
dgGoogol t2 ((Ap0 * Bp0) % p1p0);
double val2 = t2.GetAproximateValue();
if (val2 < double (0.0f)) {
return dgBigVector (float (0.0f), float (0.0f), float (0.0f), float (-1.0f));
}
dgGoogol sum = t0 + t1 + t2;
double den = sum.GetAproximateValue();
return dgBigVector (val0 / den, val1 / den, val2 / den, float (0.0f));
}
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;
}
void Path::addArcTo(const FloatPoint& p1, const FloatPoint& p2, float radius)
{
if (isEmpty())
return;
cairo_t* cr = platformPath()->context();
double x0, y0;
cairo_get_current_point(cr, &x0, &y0);
FloatPoint p0(x0, y0);
// Draw only a straight line to p1 if any of the points are equal or the radius is zero
// or the points are collinear (triangle that the points form has area of zero value).
if ((p1.x() == p0.x() && p1.y() == p0.y()) || (p1.x() == p2.x() && p1.y() == p2.y()) || !radius
|| !areaOfTriangleFormedByPoints(p0, p1, p2)) {
cairo_line_to(cr, p1.x(), p1.y());
return;
}
FloatPoint p1p0((p0.x() - p1.x()),(p0.y() - p1.y()));
FloatPoint p1p2((p2.x() - p1.x()),(p2.y() - p1.y()));
float p1p0_length = sqrtf(p1p0.x() * p1p0.x() + p1p0.y() * p1p0.y());
float p1p2_length = sqrtf(p1p2.x() * p1p2.x() + p1p2.y() * p1p2.y());
double cos_phi = (p1p0.x() * p1p2.x() + p1p0.y() * p1p2.y()) / (p1p0_length * p1p2_length);
// all points on a line logic
if (cos_phi == -1) {
cairo_line_to(cr, p1.x(), p1.y());
return;
}
if (cos_phi == 1) {
// add infinite far away point
unsigned int max_length = 65535;
double factor_max = max_length / p1p0_length;
FloatPoint ep((p0.x() + factor_max * p1p0.x()), (p0.y() + factor_max * p1p0.y()));
cairo_line_to(cr, ep.x(), ep.y());
return;
}
float tangent = radius / tan(acos(cos_phi) / 2);
float factor_p1p0 = tangent / p1p0_length;
FloatPoint t_p1p0((p1.x() + factor_p1p0 * p1p0.x()), (p1.y() + factor_p1p0 * p1p0.y()));
FloatPoint orth_p1p0(p1p0.y(), -p1p0.x());
float orth_p1p0_length = sqrt(orth_p1p0.x() * orth_p1p0.x() + orth_p1p0.y() * orth_p1p0.y());
float factor_ra = radius / orth_p1p0_length;
// angle between orth_p1p0 and p1p2 to get the right vector orthographic to p1p0
double cos_alpha = (orth_p1p0.x() * p1p2.x() + orth_p1p0.y() * p1p2.y()) / (orth_p1p0_length * p1p2_length);
if (cos_alpha < 0.f)
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
FloatPoint p((t_p1p0.x() + factor_ra * orth_p1p0.x()), (t_p1p0.y() + factor_ra * orth_p1p0.y()));
// calculate angles for addArc
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
float sa = acos(orth_p1p0.x() / orth_p1p0_length);
if (orth_p1p0.y() < 0.f)
sa = 2 * piDouble - sa;
// anticlockwise logic
bool anticlockwise = false;
float factor_p1p2 = tangent / p1p2_length;
FloatPoint t_p1p2((p1.x() + factor_p1p2 * p1p2.x()), (p1.y() + factor_p1p2 * p1p2.y()));
FloatPoint orth_p1p2((t_p1p2.x() - p.x()),(t_p1p2.y() - p.y()));
float orth_p1p2_length = sqrtf(orth_p1p2.x() * orth_p1p2.x() + orth_p1p2.y() * orth_p1p2.y());
float ea = acos(orth_p1p2.x() / orth_p1p2_length);
if (orth_p1p2.y() < 0)
ea = 2 * piDouble - ea;
if ((sa > ea) && ((sa - ea) < piDouble))
anticlockwise = true;
if ((sa < ea) && ((ea - sa) > piDouble))
anticlockwise = true;
cairo_line_to(cr, t_p1p0.x(), t_p1p0.y());
addArc(p, radius, sa, ea, anticlockwise);
}
void Path::addArcTo(const FloatPoint& p1, const FloatPoint& p2, float radius)
{
FloatPoint p0(m_path->currentPosition());
if ((p1.x() == p0.x() && p1.y() == p0.y()) || (p1.x() == p2.x() && p1.y() == p2.y()) || radius == 0.f) {
m_path->lineTo(p1);
return;
}
FloatPoint p1p0((p0.x() - p1.x()),(p0.y() - p1.y()));
FloatPoint p1p2((p2.x() - p1.x()),(p2.y() - p1.y()));
float p1p0_length = sqrtf(p1p0.x() * p1p0.x() + p1p0.y() * p1p0.y());
float p1p2_length = sqrtf(p1p2.x() * p1p2.x() + p1p2.y() * p1p2.y());
double cos_phi = (p1p0.x() * p1p2.x() + p1p0.y() * p1p2.y()) / (p1p0_length * p1p2_length);
// all points on a line logic
if (cos_phi == -1) {
m_path->lineTo(p1);
return;
}
if (cos_phi == 1) {
// add infinite far away point
unsigned int max_length = 65535;
double factor_max = max_length / p1p0_length;
FloatPoint ep((p0.x() + factor_max * p1p0.x()), (p0.y() + factor_max * p1p0.y()));
m_path->lineTo(ep);
return;
}
float tangent = radius / tan(acos(cos_phi) / 2);
float factor_p1p0 = tangent / p1p0_length;
FloatPoint t_p1p0((p1.x() + factor_p1p0 * p1p0.x()), (p1.y() + factor_p1p0 * p1p0.y()));
FloatPoint orth_p1p0(p1p0.y(), -p1p0.x());
float orth_p1p0_length = sqrt(orth_p1p0.x() * orth_p1p0.x() + orth_p1p0.y() * orth_p1p0.y());
float factor_ra = radius / orth_p1p0_length;
// angle between orth_p1p0 and p1p2 to get the right vector orthographic to p1p0
double cos_alpha = (orth_p1p0.x() * p1p2.x() + orth_p1p0.y() * p1p2.y()) / (orth_p1p0_length * p1p2_length);
if (cos_alpha < 0.f)
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
FloatPoint p((t_p1p0.x() + factor_ra * orth_p1p0.x()), (t_p1p0.y() + factor_ra * orth_p1p0.y()));
// calculate angles for addArc
orth_p1p0 = FloatPoint(-orth_p1p0.x(), -orth_p1p0.y());
float sa = acos(orth_p1p0.x() / orth_p1p0_length);
if (orth_p1p0.y() < 0.f)
sa = 2 * piDouble - sa;
// anticlockwise logic
bool anticlockwise = false;
float factor_p1p2 = tangent / p1p2_length;
FloatPoint t_p1p2((p1.x() + factor_p1p2 * p1p2.x()), (p1.y() + factor_p1p2 * p1p2.y()));
FloatPoint orth_p1p2((t_p1p2.x() - p.x()),(t_p1p2.y() - p.y()));
float orth_p1p2_length = sqrtf(orth_p1p2.x() * orth_p1p2.x() + orth_p1p2.y() * orth_p1p2.y());
float ea = acos(orth_p1p2.x() / orth_p1p2_length);
if (orth_p1p2.y() < 0)
ea = 2 * piDouble - ea;
if ((sa > ea) && ((sa - ea) < piDouble))
anticlockwise = true;
if ((sa < ea) && ((ea - sa) > piDouble))
anticlockwise = true;
m_path->lineTo(t_p1p0);
addArc(p, radius, sa, ea, anticlockwise);
}
void dPluginCamera::DrawGizmo(dSceneRender* const render, int font) const
{
render->DisableZbuffer();
//render->EnableZbuffer();
render->EnableBackFace();
render->DisableLighting ();
render->DisableTexture();
render->DisableBlend();
// calculate gizmo size
dFloat zbuffer = 0.5f;
// calculate a point the lower left corner of the screen at the front plane in global space
dFloat x0 = 40.0f;
dFloat y0 = render->GetViewPortHeight() - 30.0f;
dVector origin (render->ScreenToGlobal(dVector (x0, y0, zbuffer, 1.0f)));
dFloat length = 30.0f;
dVector p1 (render->ScreenToGlobal(dVector (x0 + length, y0, zbuffer, 1.0f)));
dVector p1p0(p1 - origin);
length = dSqrt (p1p0.DotProduct3(p1p0));
// display x axis
{
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (1.0f, 0.0f, 0.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "x");
render->PopMatrix();
}
// display y axis
{
dMatrix matrix (dRollMatrix((90.0f * 3.141592f / 180.0f)));
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (0.0f, 1.0f, 0.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "y");
render->PopMatrix();
}
// display z axis
{
dMatrix matrix (dYawMatrix((-90.0f * 3.141592f / 180.0f)));
matrix.m_posit = origin;
render->PushMatrix(matrix);
render->BeginLine();
render->SetColor(dVector (0.0f, 0.0f, 1.0f, 0.0f));
render->DrawLine (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (length, 0.0f, 0.0f, 0.0f));
render->End();
dVector posit (render->GlobalToScreen(dVector (length * 1.2f, 0.0f, 0.0f, 0.0f)));
render->SetColor(dVector (1.0f, 1.0f, 1.0f, 0.0f));
render->Print(font, posit.m_x, posit.m_y, "z");
render->PopMatrix();
}
}
