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));
}
public int Contacts(ContactInfo contacts, int maxContacts, ref idVec3 start, ref idVec6 dir, float depth, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// same as Translation but instead of storing the first collision we store all collisions as contacts
this.getContacts = true;
this.contacts = contacts;
this.maxContacts = maxContacts;
this.numContacts = 0;
idVec3 end = start + dir.SubVec3(0) * depth;
Trace results;
Translation(out results, start, end, trm, trmAxis, contentMask, model, origin, modelAxis);
if (dir.SubVec3(1).LengthSqr() != 0.0f)
{
// FIXME: rotational contacts
}
this.getContacts = false;
this.maxContacts = 0;
return this.numContacts;
}
int PointContents(idVec3 p, CmHandle model)
{
int i;
Node node = PointNode(p, this.models[(int)model]);
for (BrushRef bref = node.brushes; bref != null; bref = bref.next)
{
Brush b = bref.b;
// test if the point is within the brush bounds
for (i = 0; i < 3; i++)
{
if (p[i] < b.bounds[0][i])
{
break;
}
if (p[i] > b.bounds[1][i])
{
break;
}
}
if (i < 3)
{
continue;
}
// test if the point is inside the brush
idPlane plane = b.planes;
for (i = 0; i < b.numPlanes; i++, plane++)
{
float d = plane.Distance(p);
if (d >= 0)
{
break;
}
}
if (i >= b.numPlanes)
{
return(b.contents);
}
}
return(0);
}
int Contents(ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
if (model < 0 || model > this.maxModels || model > MAX_SUBMODELS)
{
common.Printf("CollisionModelManager::Contents: invalid model handle\n");
return 0;
}
if (this.models == null || this.models[model] == null)
{
common.Printf("CollisionModelManager::Contents: invalid model\n");
return 0;
}
Trace results;
return ContentsTrm(out results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis);
}
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 PointContents(idVec3 p, CmHandle model)
{
int i;
Node node = PointNode(p, this.models[(int)model]);
for (BrushRef bref = node.brushes; bref != null; bref = bref.next)
{
Brush b = bref.b;
// test if the point is within the brush bounds
for (i = 0; i < 3; i++)
{
if (p[i] < b.bounds[0][i])
break;
if (p[i] > b.bounds[1][i])
break;
}
if (i < 3)
continue;
// test if the point is inside the brush
idPlane plane = b.planes;
for (i = 0; i < b.numPlanes; i++, plane++)
{
float d = plane.Distance(p);
if (d >= 0)
break;
}
if (i >= b.numPlanes)
return b.contents;
}
return 0;
}
int Contents(ref idVec3 start, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 modelOrigin, ref idMat3 modelAxis)
{
if (model < 0 || model > this.maxModels || model > MAX_SUBMODELS)
{
common.Printf("CollisionModelManager::Contents: invalid model handle\n");
return(0);
}
if (this.models == null || this.models[model] == null)
{
common.Printf("CollisionModelManager::Contents: invalid model\n");
return(0);
}
Trace results;
return(ContentsTrm(out results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis));
}
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);
}
public int Contacts(ContactInfo contacts, int maxContacts, ref idVec3 start, ref idVec6 dir, float depth, idTraceModel trm, ref idMat3 trmAxis, int contentMask, CmHandle model, ref idVec3 origin, ref idMat3 modelAxis)
{
// same as Translation but instead of storing the first collision we store all collisions as contacts
this.getContacts = true;
this.contacts = contacts;
this.maxContacts = maxContacts;
this.numContacts = 0;
idVec3 end = start + dir.SubVec3(0) * depth;
Trace results;
Translation(out results, start, end, trm, trmAxis, contentMask, model, origin, modelAxis);
if (dir.SubVec3(1).LengthSqr() != 0.0f)
{
// FIXME: rotational contacts
}
this.getContacts = false;
this.maxContacts = 0;
return(this.numContacts);
}
