void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
dVector3 dv[3];
gim_trimesh_locks_work_data(&Geom->m_collision_trimesh);
gim_trimesh_get_triangle_vertices(&Geom->m_collision_trimesh, Index, dv[0],dv[1],dv[2]);
GetPointFromBarycentric(dv, u, v, Out);
gim_trimesh_unlocks_work_data(&Geom->m_collision_trimesh);
}
void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
dxTriMesh* Geom = (dxTriMesh*)g;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
dVector3 dv[3];
FetchTriangle(Geom, Index, Position, Rotation, dv);
GetPointFromBarycentric(dv, u, v, Out);
}
bool CBezierPatch::intersect(const CRay &ray, CIntersactionInfo &info, CVector3DF barCoord, unsigned int iterations, bool bDebug) const
{
//Planes along ray
CVector3DF N1 = ray.Direction().Cross(CVector3DF(-1.0f,-1.0f,-1.0f));
CVector3DF N2 = ray.Direction().Cross(N1);
const float d1 = -N1.Dot(ray.StartPoint());
const float d2 = -N2.Dot(ray.StartPoint());
float fSMall = 1e-3;
CVector3DF dBu, dBv;
for (unsigned int i = 0; i < iterations; i++)
{
const double &u(barCoord.X());
const double &v(barCoord.Y());
//partial U derivative of B
dBu = GetdBu(u,v);
//Partial V derivative of B
dBv = GetdBv(u,v);
const CVector3DF B = GetPointFromBarycentric(barCoord);
const CVector3DF R = CVector3DF(N1.Dot(B) + d1,
N2.Dot(B) + d2,
0);
if ( ( fabs(R.X()) < fSMall ) &&
( fabs(R.Y()) < fSMall ) )
{
break;
}
//Inverse Jacobian
const float fN1dotdBu = N1.Dot(dBu);
const float fN1dotdBv = N1.Dot(dBv);
const float fN2dotdBu = N2.Dot(dBu);
const float fN2dotdBv = N2.Dot(dBv);
const float invConst = 1.0f / ( fN1dotdBu * fN2dotdBv - fN1dotdBv * fN2dotdBu );
Matrix inverseJacobian;
inverseJacobian[0][0] = fN2dotdBv * invConst;
inverseJacobian[0][1] = -fN2dotdBu * invConst;
inverseJacobian[0][2] = 0;
inverseJacobian[1][0] = -fN1dotdBv * invConst;
inverseJacobian[1][1] = fN1dotdBu * invConst;
inverseJacobian[1][2] = 0;
inverseJacobian[2][0] = 0;
inverseJacobian[2][1] = 0;
inverseJacobian[2][2] = 1;
//Newton Iteration
barCoord = barCoord - R.MatrixMultiply(inverseJacobian);
barCoord.SetZ(1.0 - barCoord.X() - barCoord.Y());
if (barCoord.X() > k_fMAX || barCoord.X() < k_fMIN
|| barCoord.Y() > k_fMAX || barCoord.Y() < k_fMIN
|| barCoord.Z() > k_fMAX || barCoord.Z() < k_fMIN)
{
return false;
}
}
const float &u(barCoord.X());
const float &v(barCoord.Y());
const float w(1.0 - u - v);
if (u < -k_fSMALL || u > 1.0f + k_fSMALL
|| v < -k_fSMALL || v > 1.0f + k_fSMALL
|| w < -k_fSMALL || w > 1.0f + k_fSMALL)
{
return false;
}
if (bDebug)
{
char str[100];
sprintf( str, "u: %4.2f v: %4.2f w: %4.2f", u, v, w);
qDebug() << str;
}
//Calculating B
const CVector3DF B = GetPointFromBarycentric(barCoord);
if ( ( fabs(N1.Dot(B) + d1) > fSMall ) ||
( fabs(N2.Dot(B) + d2) > fSMall ) )
{
if (bDebug)
{
qDebug() << "error too big";
}
return false;
}
// calculate the intersection
float len = (B - ray.StartPoint()).Length();
if (len > info.m_fDistance)
{
if (bDebug)
{
qDebug() << "Len > Distance";
}
return false;
}
info.m_vIntersectionPoint = B;
info.m_fDistance = len;
info.m_vBarCoordsLocal.SetX(u);
info.m_vBarCoordsLocal.SetY(v);
info.m_vBarCoordsLocal.SetZ(w);
info.m_vBarCoordsGlobal = info.m_vBarCoordsLocal;
if (GetSettings()->m_bNormalSmoothing)
{
info.m_vNormal = GetSmoothedNormal(barCoord);
}
else
{
// info.m_vNormal = GetSmoothedNormal(barCoord);
info.m_vNormal = CVector3DF::Normal(dBu, dBv);
}
if (bDebug)
{
qDebug()<< "Patch Hit!";
}
return true;
}
const CVector3DF CBezierPatch::GetPointFromBarycentric(const QVector2D& vCoords) const
{
return GetPointFromBarycentric(vCoords.x(), vCoords.y());
}
const CVector3DF CBezierPatch::GetPointFromBarycentric(const CVector3DF& vCoords) const
{
return GetPointFromBarycentric(vCoords.X(), vCoords.Y());
}
