void dCustomPulley::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector veloc0(0.0f);
dVector veloc1(0.0f);
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// set the linear part of Jacobian 0 to translational pin vector
dVector dir0 (matrix0.m_front.Scale (1.0f/m_gearRatio));
const dVector& dir1 = matrix1.m_front;
jacobian0[0] = dir0.m_x;
jacobian0[1] = dir0.m_y;
jacobian0[2] = dir0.m_z;
jacobian0[3] = 0.0f;
jacobian0[4] = 0.0f;
jacobian0[5] = 0.0f;
jacobian1[0] = dir1.m_x;
jacobian1[1] = dir1.m_y;
jacobian1[2] = dir1.m_z;
jacobian1[3] = 0.0f;
jacobian1[4] = 0.0f;
jacobian1[5] = 0.0f;
// calculate the angular velocity for both bodies
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
// get angular velocity relative to the pin vector
dFloat w0 = veloc0.DotProduct3(dir0);
dFloat w1 = veloc1.DotProduct3(dir1);
dFloat relVeloc = w0 + w1;
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
dFloat relAccel = -0.5f * relVeloc * invTimestep;
// add a angular constraint
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
// set the desired angular acceleration between the two bodies
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
void plCullTree::IVisPolyEdge(const hsPoint3& p0, const hsPoint3& p1, hsBool dark) const
{
hsColorRGBA color;
if( dark )
color.Set(0.2f, 0.2f, 0.2f, 1.f);
else
color.Set(1.f, 1.f, 1.f, 1.f);
int vertStart = fVisVerts.GetCount();
hsVector3 dir0(&p0, &fViewPos);
hsFastMath::NormalizeAppr(dir0);
dir0 *= fVisYon;
hsVector3 dir1(&p1, &fViewPos);
hsFastMath::NormalizeAppr(dir1);
dir1 *= fVisYon;
hsPoint3 p3 = fViewPos;
p3 += dir0;
hsPoint3 p2 = fViewPos;
p2 += dir1;
hsVector3 norm = hsVector3(&p0, &fViewPos) % hsVector3(&p1, &fViewPos);
hsFastMath::NormalizeAppr(norm);
fVisVerts.Append(p0);
fVisNorms.Append(norm);
fVisColors.Append(color);
fVisVerts.Append(p1);
fVisNorms.Append(norm);
fVisColors.Append(color);
fVisVerts.Append(p2);
fVisNorms.Append(norm);
fVisColors.Append(color);
fVisVerts.Append(p3);
fVisNorms.Append(norm);
fVisColors.Append(color);
fVisTris.Append(vertStart + 0);
fVisTris.Append(vertStart + 2);
fVisTris.Append(vertStart + 1);
fVisTris.Append(vertStart + 0);
fVisTris.Append(vertStart + 3);
fVisTris.Append(vertStart + 2);
}
void CTriangle::Set(
const SVector4& point0,
const SVector4& point1,
const SVector4& point2)
{
pt[0].Set(point0.x, point0.y, point0.z);
pt[1].Set(point1.x, point1.y, point1.z);
pt[2].Set(point2.x, point2.y, point2.z);
CVector4 dir0(pt[1], pt[0], CVector4::INIT_SUB);
CVector4 dir1(pt[2], pt[0], CVector4::INIT_SUB);
SVector4::Cross(nml, dir0, dir1);
SVector4::Normalize(nml, nml);
d = -(a * pt[0].x + b * pt[0].y + c * pt[0].z);
}
dgInt32 dgCollisionTaperedCapsule::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgInt32 count = 0;
dgVector p0 (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector dir0 (p0 - origin);
dgFloat32 dist0 = dir0 % normal;
if ((dist0 * dist0) < (m_radio0 * m_radio0)) {
contactsOut[count] = p0 - normal.Scale3 (dist0);
count ++;
}
dgVector p1 (-m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector dir1 (p1 - origin);
dgFloat32 dist1 = dir1 % normal;
if ((dist1 * dist1) < (m_radio1 * m_radio1)) {
contactsOut[count] = p1 - normal.Scale3 (dist1);
count ++;
}
return count;
}
void dCustomRackAndPinion::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
dVector omega0(0.0f);
dVector veloc1(0.0f);
dFloat jacobian0[6];
dFloat jacobian1[6];
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// calculate the angular velocity for both bodies
NewtonBodyGetOmega(m_body0, &omega0[0]);
NewtonBodyGetVelocity(m_body1, &veloc1[0]);
dVector dir0 (matrix0.m_front.Scale (m_gearRatio));
const dVector& dir1 = matrix1.m_front;
jacobian0[0] = dFloat(0.0f);
jacobian0[1] = dFloat(0.0f);
jacobian0[2] = dFloat(0.0f);
jacobian0[3] = dir0.m_x;
jacobian0[4] = dir0.m_y;
jacobian0[5] = dir0.m_z;
jacobian1[0] = dir1.m_x;
jacobian1[1] = dir1.m_y;
jacobian1[2] = dir1.m_z;
jacobian1[3] = dFloat(0.0f);
jacobian1[4] = dFloat(0.0f);
jacobian1[5] = dFloat(0.0f);
dFloat w0 = omega0.DotProduct3(dir0);
dFloat w1 = veloc1.DotProduct3(dir1);
dFloat relOmega = w0 + w1;
dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep : 1.0f;
dFloat relAccel = -0.5f * relOmega * invTimestep;
NewtonUserJointAddGeneralRow (m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
}
dgVector dgBallConstraint::GetJointOmega () const
{
dgAssert (m_body0);
dgAssert (m_body1);
const dgMatrix& matrix = m_body0->GetMatrix();
dgVector dir0 (matrix.RotateVector (m_localMatrix0[0]));
dgVector dir1 (matrix.RotateVector (m_localMatrix0[1]));
dgVector dir2 (matrix.RotateVector (m_localMatrix0[2]));
const dgVector& omega0 = m_body0->GetOmega();
const dgVector& omega1 = m_body1->GetOmega();
// dgVector omega1 (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
// if (m_body1) {
// omega1 = m_body1->GetOmega();
// }
dgVector relOmega (omega0 - omega1);
return dgVector (relOmega % dir0, relOmega % dir1, relOmega % dir2, dgFloat32 (0.0f));
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert(proxy.m_referenceCollision->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_floatingCollision->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const polygonInstance = proxy.m_floatingCollision;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
dgInt32 count = 0;
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert(m_normal.m_w == dgFloat32(0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
dgVector savedFaceNormal(m_normal);
dgVector savedPosit (proxy.m_matrix.m_posit);
proxy.m_matrix.m_posit = dgVector::m_wOne;
dgVector hullOrigin(proxy.m_matrix.UnrotateVector (savedPosit));
for (dgInt32 i = 0; i < m_count; i++) {
m_localPoly[i] = hullOrigin + polyInstanceScale.CompProduct4(dgVector(&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert(m_localPoly[i].m_w == dgFloat32(0.0f));
}
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgVector normalInHull(proxy.m_matrix.RotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(proxy.m_matrix.UntransformVector(pointInHull));
dgVector p1(proxy.m_matrix.UntransformVector(hull->SupportVertex(normalInHull, NULL)));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
contactJoint->m_closestDistance = -penetration;
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector n(m_normal * e);
//dgPlane plane(n, -(m_localPoly[i0] % n));
dgPlane plane(n, - m_localPoly[i0].DotProduct4 (n).GetScalar());
plane = proxy.m_matrix.TransformPlane(plane);
//dgFloat32 supportDist = dgAbsf(plane.m_x) * boxSize.m_x + dgAbsf(plane.m_y) * boxSize.m_y + dgAbsf(plane.m_z) * boxSize.m_z;
//dgFloat32 centerDist = plane.Evalue(boxOrigin);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
proxy.m_matrix.m_posit = savedPosit;
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector pointsContacts[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, pointsContacts, 1.0f);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
dgVector globalNormal(worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = worldMatrix.TransformVector(pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*)hull->m_childShape;
count = convexShape->CalculateConvexToConvexContact(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal(polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if (normal.DotProduct4(savedFaceNormal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n(&m_vertex[index * m_stride]);
if ((savedFaceNormal.DotProduct4(n).GetScalar() > dgFloat32(0.9995f))) {
normal = n;
} else {
dgVector dir0(n * savedFaceNormal);
dgVector dir1(n * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = n;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
proxy.m_matrix.m_posit = savedPosit;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale)
{
dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI));
dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI));
const dgCollisionInstance* const hull = proxy.m_referenceCollision;
dgAssert (this == proxy.m_floatingCollision->GetChildShape());
dgAssert (m_count);
dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgBody* const floatingBody = proxy.m_floatingBody;
const dgBody* const referenceBody = proxy.m_referenceBody;
dgContact* const contactJoint = proxy.m_contactJoint;
contactJoint->m_closestDistance = dgFloat32 (1.0e10f);
m_normal = m_normal.CompProduct4(polyInstanceInvScale);
dgAssert (m_normal.m_w == dgFloat32 (0.0f));
m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt());
const dgVector savedFaceNormal (m_normal);
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride]));
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
}
dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f))));
hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne;
dgMatrix polygonMatrix;
polygonMatrix[0] = m_localPoly[1] - m_localPoly[0];
polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt());
polygonMatrix[1] = m_normal;
polygonMatrix[2] = polygonMatrix[0] * m_normal;
polygonMatrix[3] = hullOrigin;
dgAssert (polygonMatrix.TestOrthogonal());
dgMatrix savedProxyMatrix (proxy.m_matrix);
proxy.m_matrix = polygonMatrix * proxy.m_matrix;
dgVector floatingVeloc (floatingBody->m_veloc);
dgVector referenceVeloc (referenceBody->m_veloc);
const dgMatrix& hullMatrix = hull->GetGlobalMatrix();
dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc));
dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc));
dgVector polyBoxP0 (dgFloat32 ( 1.0e15f));
dgVector polyBoxP1 (dgFloat32 (-1.0e15f));
m_normal = polygonMatrix.UnrotateVector(m_normal);
if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
for (dgInt32 i = 0; i < m_count; i ++) {
m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]);
dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f));
polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]);
polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]);
}
dgInt32 count = 0;
dgVector hullBoxP0;
dgVector hullBoxP1;
hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1);
dgVector minBox (polyBoxP0 - hullBoxP1);
dgVector maxBox (polyBoxP1 - hullBoxP0);
dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc);
dgFloat32 distance = ray.BoxIntersect(minBox, maxBox);
if (distance < dgFloat32 (1.0f)) {
dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f)));
// boxOrigin += polyRelativeVeloc.Scale4 (distance);
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal;
dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal);
dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane));
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector e (m_localPoly[i] - m_localPoly[i0]);
dgVector n (m_normal * e);
dgPlane plane (n, - (m_localPoly[i0] % n));
dgVector supportDist (plane.Abs().DotProduct4 (boxSize));
dgFloat32 centerDist = plane.Evalue(boxOrigin);
if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) {
proxy.m_matrix = savedProxyMatrix;
return 0;
}
if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) {
inside = false;
}
i0 = i;
}
// for the time being for the minkousky contact calculation
inside = false;
const dgInt32 hullId = hull->GetUserDataID();
if (inside) {
dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f))));
dgVector pointInHull (hull->SupportVertex (normalInHull, NULL));
dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull));
dgFloat32 timetoImpact = dgFloat32 (0.0f);
//dgFloat32 closestDistance = dgFloat32 (0.0f);
dgAssert (0);
// dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION;
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32 (0.0f)) {
timetoImpact = penetration / (polyRelativeVeloc % m_normal);
dgAssert (timetoImpact >= dgFloat32 (0.0f));
// closestDistance = -penetration;
}
if (timetoImpact <= proxy.m_timestep) {
dgVector pointsContacts[64];
contactJoint->m_closestDistance = penetration;
dgAssert (0);
// dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector point (pointInHull);
count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f);
dgAssert (0);
// dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION));
dgVector step (hullRelativeVeloc.Scale3 (timetoImpact));
penetration = dgMax (penetration, dgFloat32 (0.0f));
const dgMatrix& worldMatrix = hull->m_globalMatrix;
dgContactPoint* const contactsOut = proxy.m_contacts;
dgVector globalNormal (worldMatrix.RotateVector(normalInHull));
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step);
contactsOut[i].m_normal = globalNormal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
}
} else {
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize ();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping (boxOrigin, convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape;
count = convexShape->CalculateConvexCastContacts (proxy);
// dgAssert (proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
#if 0
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal));
if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector n (&m_vertex[index * m_stride]);
dgVector dir0 (n * savedFaceNormal);
dgVector dir1 (n * normal);
dgFloat32 projection = dir0 % dir1;
if (projection <= dgFloat32 (0.0f)) {
normal = n;
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_normal = normal;
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i ++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
#endif
for (dgInt32 i = 0; i < count; i ++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
proxy.m_matrix = savedProxyMatrix;
return count;
}
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullDescrete(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy)
{
dgInt32 count = 0;
dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI));
dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI));
dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity());
const dgCollisionInstance* const polygonInstance = proxy.m_instance1;
dgAssert(this == polygonInstance->GetChildShape());
dgAssert(m_count);
dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0])));
const dgMatrix& hullMatrix = proxy.m_instance0->m_globalMatrix;
dgContact* const contactJoint = proxy.m_contactJoint;
const dgCollisionInstance* const hull = proxy.m_instance0;
dgVector normalInHull(hullMatrix.UnrotateVector(m_normal));
dgVector pointInHull(hull->SupportVertex(normalInHull.Scale4(dgFloat32(-1.0f)), NULL));
dgVector p0(hullMatrix.TransformVector(pointInHull));
dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness;
if (penetration < dgFloat32(0.0f)) {
return 0;
}
dgVector p1(hullMatrix.TransformVector(hull->SupportVertex(normalInHull, NULL)));
contactJoint->m_closestDistance = dgFloat32(0.0f);
dgFloat32 distance = (m_localPoly[0] - p1) % m_normal;
if (distance >= dgFloat32(0.0f)) {
return 0;
}
dgVector boxSize (hull->GetBoxSize() & dgVector::m_triplexMask);
dgVector boxOrigin ((hull->GetBoxOrigin() & dgVector::m_triplexMask) + dgVector::m_wOne);
bool inside = true;
dgInt32 i0 = m_count - 1;
for (dgInt32 i = 0; i < m_count; i++) {
dgVector e(m_localPoly[i] - m_localPoly[i0]);
dgVector edgeBoundaryNormal(m_normal * e);
dgPlane plane(edgeBoundaryNormal, - m_localPoly[i0].DotProduct4 (edgeBoundaryNormal).GetScalar());
plane = hullMatrix.TransformPlane(plane);
dgFloat32 supportDist = boxSize.DotProduct4 (plane.Abs()).GetScalar();
dgFloat32 centerDist = plane.DotProduct4 (boxOrigin).GetScalar();
if ((centerDist + supportDist) < dgFloat32(0.0f)) {
return 0;
}
if ((centerDist - supportDist) < dgFloat32(0.0f)) {
inside = false;
break;
}
i0 = i;
}
//inside = false;
dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize();
if (m_faceClipSize > convexSphapeUmbra) {
BeamClipping(dgVector(dgFloat32(0.0f)), convexSphapeUmbra);
m_faceClipSize = hull->m_childShape->GetBoxMaxRadius();
}
const dgInt32 hullId = hull->GetUserDataID();
if (inside & !proxy.m_intersectionTestOnly) {
dgAssert(penetration >= dgFloat32(0.0f));
dgVector contactPoints[64];
dgAssert(penetration >= 0.0f);
dgVector point(pointInHull + normalInHull.Scale4(penetration + DG_ROBUST_PLANE_CLIP));
count = hull->CalculatePlaneIntersection(normalInHull.Scale4(dgFloat32(-1.0f)), point, contactPoints);
dgVector step(normalInHull.Scale4((proxy.m_skinThickness - penetration) * dgFloat32(0.5f)));
dgContactPoint* const contactsOut = proxy.m_contacts;
dgAssert(contactsOut);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_point = hullMatrix.TransformVector(contactPoints[i] + step);
contactsOut[i].m_normal = m_normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
contactsOut[i].m_penetration = penetration;
}
} else {
m_vertexCount = dgUnsigned16 (m_count);
count = world->CalculateConvexToConvexContacts(proxy);
dgAssert(proxy.m_intersectionTestOnly || (count >= 0));
if (count >= 1) {
dgContactPoint* const contactsOut = proxy.m_contacts;
if (m_closestFeatureType == 3) {
for (dgInt32 i = 0; i < count; i++) {
//contactsOut[i].m_userId = m_faceId;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
dgVector normal (contactsOut[0].m_normal);
if (normal.DotProduct4(m_normal).GetScalar() < dgFloat32(0.9995f)) {
dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex];
dgVector adjacentNormal (CalculateGlobalNormal (parentMesh, dgVector(&m_vertex[index * m_stride])));
if ((m_normal.DotProduct4(adjacentNormal).GetScalar() > dgFloat32(0.9995f))) {
normal = adjacentNormal;
} else {
dgVector dir0(adjacentNormal * m_normal);
dgVector dir1(adjacentNormal * normal);
dgFloat32 projection = dir0.DotProduct4(dir1).GetScalar();
if (projection <= dgFloat32(0.0f)) {
normal = adjacentNormal;
}
}
normal = polygonInstance->m_globalMatrix.RotateVector(normal);
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_normal = normal;
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
} else {
for (dgInt32 i = 0; i < count; i++) {
contactsOut[i].m_shapeId0 = hullId;
contactsOut[i].m_shapeId1 = m_faceId;
}
}
}
}
}
return count;
}
void GenericContactProcess (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
#if 0
dFloat speed0;
dFloat speed1;
SpecialEffectStruct* currectEffect;
// get the pointer to the special effect structure
currectEffect = (SpecialEffectStruct *)NewtonMaterialGetMaterialPairUserData (material);
// save the contact information
NewtonMaterialGetContactPositionAndNormal (material, &currectEffect->m_position.m_x, &currectEffect->m_normal.m_x);
NewtonMaterialGetContactTangentDirections (material, &currectEffect->m_tangentDir0.m_x, &currectEffect->m_tangentDir1.m_x);
// Get the maximum normal speed of this impact. this can be used for positioning collision sound
speed0 = NewtonMaterialGetContactNormalSpeed (material);
if (speed0 > currectEffect->m_contactMaxNormalSpeed) {
// save the position of the contact (for 3d sound of particles effects)
currectEffect->m_contactMaxNormalSpeed = speed0;
}
// get the maximum of the two sliding contact speed
speed0 = NewtonMaterialGetContactTangentSpeed (material, 0);
speed1 = NewtonMaterialGetContactTangentSpeed (material, 1);
if (speed1 > speed0) {
speed0 = speed1;
}
// Get the maximum tangent speed of this contact. this can be used for particles(sparks) of playing scratch sounds
if (speed0 > currectEffect->m_contactMaxTangentSpeed) {
// save the position of the contact (for 3d sound of particles effects)
currectEffect->m_contactMaxTangentSpeed = speed0;
}
#endif
// read the table direction
// dVector dir (tableDir);
// dVector updir (TableDir);
// NewtonBody* const body = NewtonJointGetBody0(contactJoint);
// for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
// dFloat speed;
// dVector point;
// dVector normal;
// dVector dir0;
// dVector dir1;
// dVector force;
// NewtonMaterial* material;
//
// material = NewtonContactGetMaterial (contact);
// NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
//
// // if the normal is vertical is large the say 40 degrees
// if (fabsf (normal % upDir) > 0.7f) {
// // rotate the normal to be aligned with the table direction
// NewtonMaterialContactRotateTangentDirections (material, dir);
// }
// }
NewtonBody* const body = NewtonJointGetBody0(contactJoint);
for (void* contact = NewtonContactJointGetFirstContact (contactJoint); contact; contact = NewtonContactJointGetNextContact (contactJoint, contact)) {
dVector point(0.0f);
dVector normal(0.0f);
dVector dir0(0.0f);
dVector dir1(0.0f);
dVector force(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial (contact);
NewtonMaterialGetContactForce (material, body, &force.m_x);
NewtonMaterialGetContactPositionAndNormal (material, body, &point.m_x, &normal.m_x);
NewtonMaterialGetContactTangentDirections (material, body, &dir0.m_x, &dir1.m_x);
//dFloat speed = NewtonMaterialGetContactNormalSpeed(material);
//speed = NewtonMaterialGetContactNormalSpeed(material);
// play sound base of the contact speed.
//
}
}
//--------------------------------------------------------------
void ofApp::setup(){
kinect.setRegistration(false);
kinect.init();
kinect.open();
kinect.setDepthClipping(950,1350); // kinect depth calcs 95cm-135cm (higher resolution)
ofBackground(0);
ofSetVerticalSync(true);
layers.resize(4);
layers[0].loadVideo("mars 1080.mp4");
layers[0].setThresholds(255, THRESH0); // near, far
layers[0].setColor(ofColor(236,109,18));
ofDirectory dir0("mars audio");
dir0.allowExt("aif");
dir0.listDir();
for (int i=0; i<dir0.size(); i++){
layers[0].loadAudio(dir0.getPath(i));
}
layers[1].setApache();
layers[1].loadVideo("apache 1080.mp4");
layers[1].setThresholds(THRESH0, THRESH1);
layers[1].setColor(ofColor(255,225,86));
layers[1].loadAudio("apache ambient.aif");
layers[2].loadVideo("fracking 1080.mp4");
layers[2].setThresholds(THRESH1, THRESH2);
layers[2].setColor(ofColor(137,210,65));
ofDirectory dir2("fracking audio");
dir2.allowExt("aif");
dir2.listDir();
for (int i=0; i<dir2.size(); i++){
layers[2].loadAudio(dir2.getPath(i));
}
layers[3].loadVideo("piri reis 1080.mp4");
layers[3].setThresholds(THRESH2, 0);
layers[3].setColor(ofColor(89,192,214));
warper.setup(0,0,640,480);
fbo.allocate(640,480,GL_RGB);
width = ofGetWidth();
height = ofGetHeight();
depthGui.setup();
depthGui.add(topSlider.setup("top", TOP, 0, 255));
depthGui.add(thresh0Slider.setup("thresh0", THRESH0, 0, 255));
depthGui.add(thresh1Slider.setup("thresh1", THRESH1, 0, 255));
depthGui.add(thresh2Slider.setup("thresh2", THRESH2, 0, 255));
depthGui.add(bottomSlider.setup("bottom", TOP, 0, 255));
depthGui.add(blurRadius.setup("blur", 4, 0, 40));
depthGui.add(lerpAmount.setup("smooth", 0.01, 0, 1.0));
// set kinect warp corners to defaults
kinectCorners[TOP_LEFT] = ofVec2f(0,0);
kinectCorners[TOP_RIGHT] = ofVec2f(640,0);
kinectCorners[BOTTOM_RIGHT] = ofVec2f(640,480);
kinectCorners[BOTTOM_LEFT] = ofVec2f(0,480);
kinectWarp.allocate(640,480,OF_IMAGE_COLOR);
kinectWarp.setColor(ofColor(0,0,0));
kinectWarp.setImageType(OF_IMAGE_GRAYSCALE);
}
void CCone::glCommands(bool select, bool marked, bool no_color)
{
if(!m_render_without_OpenCASCADE)
{
CSolid::glCommands(select, marked, no_color);
return;
}
glEnable(GL_LIGHTING);
glShadeModel(GL_SMOOTH);
Material(m_color).glMaterial(1.0);
int num_facets = 20;
gp_Pnt pt_top = m_pos.Location().XYZ() + m_pos.Direction().XYZ() * m_height;
for(int angle_pos = 0; angle_pos<num_facets; angle_pos++)
{
double angle0 = ((double)angle_pos)/num_facets * 6.2831853071795;
double angle1 = ((double)(angle_pos + 1))/num_facets * 6.2831853071795;
gp_Dir dir0(m_pos.XDirection().XYZ() * cos(angle0) + m_pos.YDirection().XYZ() * sin(angle0));
gp_Dir dir1(m_pos.XDirection().XYZ() * cos(angle1) + m_pos.YDirection().XYZ() * sin(angle1));
double p[4][3];
extract(m_pos.Location().XYZ() + dir0.XYZ() * m_temp_r1, p[0]);
extract(m_pos.Location().XYZ() + dir1.XYZ() * m_temp_r1, p[1]);
extract(pt_top.XYZ() + dir1.XYZ() * m_temp_r2, p[2]);
extract(pt_top.XYZ() + dir0.XYZ() * m_temp_r2, p[3]);
gp_Dir upside0(p[3][0] - p[0][0], p[3][1] - p[0][1], p[3][2] - p[0][2]);
gp_Dir upside1(p[2][0] - p[1][0], p[2][1] - p[1][1], p[2][2] - p[1][2]);
gp_Dir around0( m_pos.YDirection().XYZ() * cos(angle0) - m_pos.XDirection().XYZ() * sin(angle0));
gp_Dir around1( m_pos.YDirection().XYZ() * cos(angle1) - m_pos.XDirection().XYZ() * sin(angle1));
gp_Dir norm0 = around0 ^ upside0;
gp_Dir norm1 = around1 ^ upside1;
double n0[3], n1[3];
extract(norm0, n0);
extract(norm1, n1);
double up[3];
extract(m_pos.Direction(), up);
double down[3] = {-up[0], -up[1], -up[2]};
double p_bot[3], p_top[3];
extract(m_pos.Location(), p_bot);
extract(pt_top, p_top);
glBegin(GL_TRIANGLES);
glNormal3dv(n0);
glVertex3dv(p[0]);
glNormal3dv(n1);
glVertex3dv(p[1]);
glNormal3dv(n1);
glVertex3dv(p[2]);
glNormal3dv(n0);
glVertex3dv(p[0]);
glNormal3dv(n1);
glVertex3dv(p[2]);
glNormal3dv(n0);
glVertex3dv(p[3]);
glNormal3dv(up);
glVertex3dv(p[3]);
glVertex3dv(p[2]);
glVertex3dv(p_top);
glNormal3dv(down);
glVertex3dv(p_bot);
glVertex3dv(p[1]);
glVertex3dv(p[0]);
glEnd();
}
glDisable(GL_LIGHTING);
glShadeModel(GL_FLAT);
glColor3ub(0, 0, 0);
glBegin(GL_LINE_STRIP);
for(int angle_pos = 0; angle_pos<=num_facets; angle_pos++)
{
double angle = ((double)angle_pos)/num_facets * 6.2831853071795;
gp_Dir dir(m_pos.XDirection().XYZ() * cos(angle) + m_pos.YDirection().XYZ() * sin(angle));
double p[3];
extract(m_pos.Location().XYZ() + dir.XYZ() * m_temp_r1, p);
glVertex3dv(p);
}
glEnd();
glBegin(GL_LINE_STRIP);
for(int angle_pos = 0; angle_pos<=num_facets; angle_pos++)
{
double angle = ((double)angle_pos)/num_facets * 6.2831853071795;
gp_Dir dir(m_pos.XDirection().XYZ() * cos(angle) + m_pos.YDirection().XYZ() * sin(angle));
double p[3];
extract(pt_top.XYZ() + dir.XYZ() * m_temp_r2, p);
glVertex3dv(p);
}
glEnd();
}
void CoronaRenderer::defineMesh(mtco_MayaObject *obj)
{
MObject meshObject = obj->mobject;
MStatus stat = MStatus::kSuccess;
bool hasDisplacement = false;
Corona::Abstract::Map *displacementMap = NULL;
float displacementMin = 0.0f;
float displacementMax = 0.01f;
// I do it here for displacement mapping, maybe we should to another place
getObjectShadingGroups(obj->dagPath, obj->perFaceAssignments, obj->shadingGroups);
if( obj->shadingGroups.length() > 0)
{
MFnDependencyNode shadingGroup(obj->shadingGroups[0]);
MString sgn = shadingGroup.name();
MObject displacementObj = getConnectedInNode(obj->shadingGroups[0], "displacementShader");
MString doo = getObjectName(displacementObj);
if( (displacementObj != MObject::kNullObj) && (displacementObj.hasFn(MFn::kDisplacementShader)))
{
MObject displacementMapObj = getConnectedInNode(displacementObj, "displacement");
if( (displacementMapObj != MObject::kNullObj) && (displacementMapObj.hasFn(MFn::kFileTexture)))
{
MFnDependencyNode displacmentMapNode(displacementObj);
getFloat("mtco_displacementMin", displacmentMapNode, displacementMin);
getFloat("mtco_displacementMax", displacmentMapNode, displacementMax);
MString fileTexturePath = getConnectedFileTexturePath(MString("displacement"), displacmentMapNode);
if( fileTexturePath != "")
{
MapLoader loader;
displacementMap = loader.loadBitmap(fileTexturePath.asChar());
hasDisplacement = true;
}
}
}
}
MFnMesh meshFn(meshObject, &stat);
CHECK_MSTATUS(stat);
MItMeshPolygon faceIt(meshObject, &stat);
CHECK_MSTATUS(stat);
MPointArray points;
meshFn.getPoints(points);
MFloatVectorArray normals;
meshFn.getNormals( normals, MSpace::kWorld );
MFloatArray uArray, vArray;
meshFn.getUVs(uArray, vArray);
//logger.debug(MString("Translating mesh object ") + meshFn.name().asChar());
MString meshFullName = makeGoodString(meshFn.fullPathName());
Corona::TriangleData td;
Corona::IGeometryGroup* geom = NULL;
geom = this->context.scene->addGeomGroup();
obj->geom = geom;
for( uint vtxId = 0; vtxId < points.length(); vtxId++)
{
geom->getVertices().push(Corona::Pos(points[vtxId].x,points[vtxId].y,points[vtxId].z));
}
for( uint nId = 0; nId < normals.length(); nId++)
{
geom->getNormals().push(Corona::Dir(normals[nId].x,normals[nId].y,normals[nId].z));
}
for( uint tId = 0; tId < uArray.length(); tId++)
{
geom->getMapCoords().push(Corona::Pos(uArray[tId],vArray[tId],0.0f));
geom->getMapCoordIndices().push(geom->getMapCoordIndices().size());
}
MPointArray triPoints;
MIntArray triVtxIds;
MIntArray faceVtxIds;
MIntArray faceNormalIds;
for(faceIt.reset(); !faceIt.isDone(); faceIt.next())
{
int faceId = faceIt.index();
int numTris;
faceIt.numTriangles(numTris);
faceIt.getVertices(faceVtxIds);
MIntArray faceUVIndices;
faceNormalIds.clear();
for( uint vtxId = 0; vtxId < faceVtxIds.length(); vtxId++)
{
faceNormalIds.append(faceIt.normalIndex(vtxId));
int uvIndex;
faceIt.getUVIndex(vtxId, uvIndex);
faceUVIndices.append(uvIndex);
}
for( int triId = 0; triId < numTris; triId++)
{
int faceRelIds[3];
faceIt.getTriangle(triId, triPoints, triVtxIds);
for( uint triVtxId = 0; triVtxId < 3; triVtxId++)
{
for(uint faceVtxId = 0; faceVtxId < faceVtxIds.length(); faceVtxId++)
{
if( faceVtxIds[faceVtxId] == triVtxIds[triVtxId])
{
faceRelIds[triVtxId] = faceVtxId;
}
}
}
uint vtxId0 = faceVtxIds[faceRelIds[0]];
uint vtxId1 = faceVtxIds[faceRelIds[1]];
uint vtxId2 = faceVtxIds[faceRelIds[2]];
uint normalId0 = faceNormalIds[faceRelIds[0]];
uint normalId1 = faceNormalIds[faceRelIds[1]];
uint normalId2 = faceNormalIds[faceRelIds[2]];
uint uvId0 = faceUVIndices[faceRelIds[0]];
uint uvId1 = faceUVIndices[faceRelIds[1]];
uint uvId2 = faceUVIndices[faceRelIds[2]];
if( hasDisplacement )
{
Corona::DisplacedTriangleData tri;
tri.displacement.map = displacementMap;
MPoint p0 = points[vtxId0];
MPoint p1 = points[vtxId1];
MPoint p2 = points[vtxId2];
tri.v[0] = Corona::AnimatedPos(Corona::Pos(p0.x, p0.y, p0.z));
tri.v[1] = Corona::AnimatedPos(Corona::Pos(p1.x, p1.y, p1.z));
tri.v[2] = Corona::AnimatedPos(Corona::Pos(p2.x, p2.y, p2.z));
MVector n0 = normals[normalId0];
MVector n1 = normals[normalId1];
MVector n2 = normals[normalId2];
Corona::Dir dir0(n0.x, n0.y, n0.z);
Corona::Dir dir1(n1.x, n1.y, n1.z);
Corona::Dir dir2(n2.x, n2.y, n2.z);
tri.n[0] = Corona::AnimatedDir(dir0);
tri.n[1] = Corona::AnimatedDir(dir1);
tri.n[2] = Corona::AnimatedDir(dir2);
Corona::Pos uv0(uArray[uvId0],vArray[uvId0],0.0);
Corona::Pos uv1(uArray[uvId1],vArray[uvId1],0.0);
Corona::Pos uv2(uArray[uvId2],vArray[uvId2],0.0);
Corona::StaticArray<Corona::Pos, 3> uvp;
uvp[0] = uv0;
uvp[1] = uv1;
uvp[2] = uv2;
tri.t.push(uvp);
tri.materialId = 0;
tri.displacement.min = displacementMin;
tri.displacement.max = displacementMax;
geom->addPrimitive(tri);
}else{
Corona::TriangleData tri;
tri.v = Corona::AnimatedPosI3(vtxId0, vtxId1, vtxId2);
tri.n = Corona::AnimatedDirI3(normalId0, normalId1, normalId2);
tri.t[0] = uvId0;
tri.t[1] = uvId1;
tri.t[2] = uvId2;
tri.materialId = 0;
geom->addPrimitive(tri);
}
}
}
}
