int TransformedPointContents(ref idVec3 p, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// subtract origin offset
idVec3 p_l = p - origin;
if (modelAxis.IsRotated())
{
p_l *= modelAxis;
}
return(PointContents(p_l, model));
}
int ContentsTrm(Trace results, ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
// fast point case
if (!trm || (trm.bounds[1][0] - trm.bounds[0][0] <= 0.0f &&
trm.bounds[1][1] - trm.bounds[0][1] <= 0.0f &&
trm.bounds[1][2] - trm.bounds[0][2] <= 0.0f))
{
results.c.contents = TransformedPointContents(start, model, modelOrigin, modelAxis);
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return results.c.contents;
}
this.checkCount++;
TraceWork tw = new TraceWork();
tw.trace.fraction = 1.0f;
tw.trace.c.contents = 0;
tw.trace.c.type = ContactType.CONTACT_NONE;
tw.contents = contentMask;
tw.isConvex = true;
tw.rotation = false;
tw.positionTest = true;
tw.pointTrace = false;
tw.quickExit = false;
tw.numContacts = 0;
tw.model = this.models[(int)model];
tw.start = start - modelOrigin;
tw.end = tw.start;
bool model_rotated = modelAxis.IsRotated();
if (model_rotated)
invModelAxis = modelAxis.Transpose();
// setup trm structure
SetupTrm(ref tw, trm);
bool trm_rotated = trmAxis.IsRotated();
// calculate vertex positions
if (trm_rotated)
for (int i = 0; i < tw.numVerts; i++)
// rotate trm around the start position
tw.vertices[i].p *= trmAxis;
for (int i = 0; i < tw.numVerts; i++)
// set trm at start position
tw.vertices[i].p += tw.start;
if (model_rotated)
for (int i = 0; i < tw.numVerts; i++)
// rotate trm around model instead of rotating the model
tw.vertices[i].p *= invModelAxis;
// add offset to start point
if (trm_rotated)
{
idVec3 dir = trm->offset * trmAxis;
tw.start += dir;
tw.end += dir;
}
else
{
tw.start += trm->offset;
tw.end += trm->offset;
}
if (model_rotated)
{
// rotate trace instead of model
tw.start *= invModelAxis;
tw.end *= invModelAxis;
}
// setup trm vertices
tw.size.Clear();
for (int i = 0; i < tw.numVerts; i++)
// get axial trm size after rotations
tw.size.AddPoint(tw.vertices[i].p - tw.start);
// setup trm edges
for (int i = 1; i <= tw.numEdges; i++)
{
// edge start, end and pluecker coordinate
tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
tw.edges[i].pl.FromLine(tw.edges[i].start, tw.edges[i].end);
}
// setup trm polygons
if (trm_rotated & model_rotated)
{
idMat3 tmpAxis = trmAxis * invModelAxis;
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= tmpAxis;
}
else if (trm_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= trmAxis;
}
else if (model_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane *= invModelAxis;
}
for (int i = 0; i < tw.numPolys; i++)
tw.polys[i].plane.FitThroughPoint(tw.edges[abs(tw.polys[i].edges[0])].start);
// bounds for full trace, a little bit larger for epsilons
for (int i = 0; i < 3; i++)
{
if (tw.start[i] < tw.end[i])
{
tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
else
{
tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
if (idMath.Fabs(tw.size[0][i]) > idMath.Fabs(tw.size[1][i]))
tw.extents[i] = idMath.Fabs(tw.size[0][i]) + CM_BOX_EPSILON;
else
tw.extents[i] = idMath.Fabs(tw.size[1][i]) + CM_BOX_EPSILON;
}
// trace through the model
TraceThroughModel(ref tw);
results = tw.trace;
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return results.c.contents;
}
int TransformedPointContents(ref idVec3 p, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// subtract origin offset
idVec3 p_l = p - origin;
if (modelAxis.IsRotated())
p_l *= modelAxis;
return PointContents(p_l, model);
}
int ContentsTrm(Trace results, ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
// fast point case
if (!trm || (trm.bounds[1][0] - trm.bounds[0][0] <= 0.0f &&
trm.bounds[1][1] - trm.bounds[0][1] <= 0.0f &&
trm.bounds[1][2] - trm.bounds[0][2] <= 0.0f))
{
results.c.contents = TransformedPointContents(start, model, modelOrigin, modelAxis);
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return(results.c.contents);
}
this.checkCount++;
TraceWork tw = new TraceWork();
tw.trace.fraction = 1.0f;
tw.trace.c.contents = 0;
tw.trace.c.type = ContactType.CONTACT_NONE;
tw.contents = contentMask;
tw.isConvex = true;
tw.rotation = false;
tw.positionTest = true;
tw.pointTrace = false;
tw.quickExit = false;
tw.numContacts = 0;
tw.model = this.models[(int)model];
tw.start = start - modelOrigin;
tw.end = tw.start;
bool model_rotated = modelAxis.IsRotated();
if (model_rotated)
{
invModelAxis = modelAxis.Transpose();
}
// setup trm structure
SetupTrm(ref tw, trm);
bool trm_rotated = trmAxis.IsRotated();
// calculate vertex positions
if (trm_rotated)
{
for (int i = 0; i < tw.numVerts; i++)
{
// rotate trm around the start position
tw.vertices[i].p *= trmAxis;
}
}
for (int i = 0; i < tw.numVerts; i++)
{
// set trm at start position
tw.vertices[i].p += tw.start;
}
if (model_rotated)
{
for (int i = 0; i < tw.numVerts; i++)
{
// rotate trm around model instead of rotating the model
tw.vertices[i].p *= invModelAxis;
}
}
// add offset to start point
if (trm_rotated)
{
idVec3 dir = trm->offset * trmAxis;
tw.start += dir;
tw.end += dir;
}
else
{
tw.start += trm->offset;
tw.end += trm->offset;
}
if (model_rotated)
{
// rotate trace instead of model
tw.start *= invModelAxis;
tw.end *= invModelAxis;
}
// setup trm vertices
tw.size.Clear();
for (int i = 0; i < tw.numVerts; i++)
{
// get axial trm size after rotations
tw.size.AddPoint(tw.vertices[i].p - tw.start);
}
// setup trm edges
for (int i = 1; i <= tw.numEdges; i++)
{
// edge start, end and pluecker coordinate
tw.edges[i].start = tw.vertices[tw.edges[i].vertexNum[0]].p;
tw.edges[i].end = tw.vertices[tw.edges[i].vertexNum[1]].p;
tw.edges[i].pl.FromLine(tw.edges[i].start, tw.edges[i].end);
}
// setup trm polygons
if (trm_rotated & model_rotated)
{
idMat3 tmpAxis = trmAxis * invModelAxis;
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= tmpAxis;
}
}
else if (trm_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= trmAxis;
}
}
else if (model_rotated)
{
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane *= invModelAxis;
}
}
for (int i = 0; i < tw.numPolys; i++)
{
tw.polys[i].plane.FitThroughPoint(tw.edges[abs(tw.polys[i].edges[0])].start);
}
// bounds for full trace, a little bit larger for epsilons
for (int i = 0; i < 3; i++)
{
if (tw.start[i] < tw.end[i])
{
tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
else
{
tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON;
tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON;
}
if (idMath.Fabs(tw.size[0][i]) > idMath.Fabs(tw.size[1][i]))
{
tw.extents[i] = idMath.Fabs(tw.size[0][i]) + CM_BOX_EPSILON;
}
else
{
tw.extents[i] = idMath.Fabs(tw.size[1][i]) + CM_BOX_EPSILON;
}
}
// trace through the model
TraceThroughModel(ref tw);
results = tw.trace;
results.fraction = (results.c.contents == 0);
results.endpos = start;
results.endAxis = trmAxis;
return(results.c.contents);
}
