public Capsule()
{
// uninitialized
mSegment = new Segment();
mRadius = float.NaN;
}
//----------------------------------------------------------------------------
public Capsule( Segment segment, float radius )
{
mSegment = segment;
mRadius = radius;
}
//----------------------------------------------------------------------------
public bool Intersects( Segment segment )
{
Real fDist = segment.Distance( mSegment );
return fDist <= mRadius;
}
//----------------------------------------------------------------------------
public float SquaredDistance( Segment otherSegment )
{
Vector3 kDiff = Origin - otherSegment.Origin;
float fA01 = -Direction.Dot( otherSegment.Direction );
float fB0 = kDiff.Dot( Direction );
float fB1 = -kDiff.Dot( otherSegment.Direction );
float fC = kDiff.LengthSquared;
float fDet = System.Math.Abs( (float)1.0 - fA01*fA01 );
float fS0, fS1, fSqrDist, fExtDet0, fExtDet1, fTmpS0, fTmpS1;
if ( fDet >= Parallel_Tolerance )
{
// segments are not parallel
fS0 = fA01*fB1 - fB0;
fS1 = fA01*fB0 - fB1;
fExtDet0 = Extent*fDet;
fExtDet1 = otherSegment.Extent*fDet;
if ( fS0 >= -fExtDet0 )
{
if ( fS0 <= fExtDet0 )
{
if ( fS1 >= -fExtDet1 )
{
if ( fS1 <= fExtDet1 ) // region 0 (interior)
{
// minimum at two interior points of 3D lines
float fInvDet = ( (float)1.0 )/fDet;
fS0 *= fInvDet;
fS1 *= fInvDet;
fSqrDist = fS0*( fS0 + fA01*fS1 + ( (float)2.0 )*fB0 ) + fS1*( fA01*fS0 + fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else // region 3 (side)
{
fS1 = otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 < -Extent )
{
fS0 = -Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 <= Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
}
}
else // region 7 (side)
{
fS1 = -otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 < -Extent )
{
fS0 = -Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 <= Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
}
}
else
{
if ( fS1 >= -fExtDet1 )
{
if ( fS1 <= fExtDet1 ) // region 1 (side)
{
fS0 = Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 < -otherSegment.Extent )
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 <= otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
else // region 2 (corner)
{
fS1 = otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 < -Extent )
{
fS0 = -Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 <= Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 < -otherSegment.Extent )
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 <= otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
}
}
else // region 8 (corner)
{
fS1 = -otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 < -Extent )
{
fS0 = -Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 <= Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 > otherSegment.Extent )
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 >= -otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
}
}
}
else
{
if ( fS1 >= -fExtDet1 )
{
if ( fS1 <= fExtDet1 ) // region 5 (side)
{
fS0 = -Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 < -otherSegment.Extent )
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 <= otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
else // region 4 (corner)
{
fS1 = otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 > Extent )
{
fS0 = Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 >= -Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = -Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 < -otherSegment.Extent )
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 <= otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
}
}
else // region 6 (corner)
{
fS1 = -otherSegment.Extent;
fTmpS0 = -( fA01*fS1 + fB0 );
if ( fTmpS0 > Extent )
{
fS0 = Extent;
fSqrDist = fS0*( fS0 - ( (float)2.0 )*fTmpS0 ) + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else if ( fTmpS0 >= -Extent )
{
fS0 = fTmpS0;
fSqrDist = -fS0*fS0 + fS1*( fS1 + ( (float)2.0 )*fB1 ) + fC;
}
else
{
fS0 = -Extent;
fTmpS1 = -( fA01*fS0 + fB1 );
if ( fTmpS1 < -otherSegment.Extent )
{
fS1 = -otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else if ( fTmpS1 <= otherSegment.Extent )
{
fS1 = fTmpS1;
fSqrDist = -fS1*fS1 + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
else
{
fS1 = otherSegment.Extent;
fSqrDist = fS1*( fS1 - ( (float)2.0 )*fTmpS1 ) + fS0*( fS0 + ( (float)2.0 )*fB0 ) + fC;
}
}
}
}
}
else
{
// The segments are parallel. The average b0 term is designed to
// ensure symmetry of the function. That is, dist(seg0,seg1) and
// dist(seg1,seg0) should produce the same number.
float fE0pE1 = Extent + otherSegment.Extent;
float fSign = ( fA01 > (float)0.0 ? (float)-1.0 : (float)1.0 );
float fB0Avr = ( (float)0.5 )*( fB0 - fSign*fB1 );
float fLambda = -fB0Avr;
if ( fLambda < -fE0pE1 )
{
fLambda = -fE0pE1;
}
else if ( fLambda > fE0pE1 )
{
fLambda = fE0pE1;
}
fS1 = -fSign*fLambda*otherSegment.Extent/fE0pE1;
fS0 = fLambda + fSign*fS1;
fSqrDist = fLambda*( fLambda + ( (float)2.0 )*fB0Avr ) + fC;
}
// we don't need the following stuff - it's for calculating closest point
// m_kClosestPoint0 = origin + fS0*direction;
// m_kClosestPoint1 = otherSegment.origin + fS1*otherSegment.direction;
// m_fSegment0Parameter = fS0;
// m_fSegment1Parameter = fS1;
return System.Math.Abs( fSqrDist );
}
public float Distance( Segment otherSegment )
{
float fSqrDist = SquaredDistance( otherSegment );
return Utility.Sqrt( fSqrDist );
}
/* Test if a scene node intersected a portal during the last time delta
* (from last frame time to current frame time). This function checks
* if the node "crossed over" the portal also.
*/
public PortalIntersectResult intersects( PCZSceneNode pczsn )
{
// Only check if portal is open
if ( mOpen )
{
if ( pczsn == mNode )
{
// ignore the scene node if it is the node the portal is associated with
return PortalIntersectResult.NO_INTERSECT;
}
// most complicated case - if the portal is a quad:
if ( mType == PORTAL_TYPE.PORTAL_TYPE_QUAD )
{
// the node is modeled as a line segment (prevPostion to currentPosition)
// intersection test is then between the capsule and the line segment.
Segment nodeSegment = new Segment();
nodeSegment.Set( pczsn.PreviousPosition, pczsn.DerivedPosition );
// we model the portal as a line swept sphere (mPrevDerivedCP to mDerivedCP).
Capsule portalCapsule = new Capsule();
portalCapsule.Set( mPrevDerivedCP, mDerivedCP, mRadius );
if ( portalCapsule.Intersects( nodeSegment ) )
{
// the portal intersected the node at some time from last frame to this frame.
// Now check if node "crossed" the portal
// a crossing occurs if the "side" of the final position of the node compared
// to the final position of the portal is negative AND the initial position
// of the node compared to the initial position of the portal is non-negative
if ( mDerivedPlane.GetSide( pczsn.DerivedPosition ) == PlaneSide.Negative &&
mPrevDerivedPlane.GetSide( pczsn.DerivedPosition ) != PlaneSide.Negative )
{
// safety check - make sure the node has at least one dimension which is
// small enough to fit through the portal! (avoid the "elephant fitting
// through a mouse hole" case)
Vector3 nodeHalfVector = pczsn.WorldAABB.HalfSize;
Vector3 portalBox = new Vector3( mRadius, mRadius, mRadius );
portalBox.Floor( nodeHalfVector );
if ( portalBox.x < mRadius )
{
// crossing occurred!
return PortalIntersectResult.INTERSECT_CROSS;
}
}
}
// there was no crossing of the portal by the node, but it might be touching
// the portal. We check for this by checking the bounding box of the node vs.
// the sphere of the portal
if ( mDerivedSphere.Intersects( pczsn.WorldAABB ) &&
mDerivedPlane.GetSide( pczsn.WorldAABB ) == PlaneSide.Both )
{
// intersection but no crossing
// note this means that the node is CURRENTLY touching the portal.
if ( mDerivedPlane.GetSide( pczsn.DerivedPosition ) != PlaneSide.Negative )
{
// the node is on the positive (front) or exactly on the CP of the portal
return PortalIntersectResult.INTERSECT_NO_CROSS;
}
else
{
// the node is on the negative (back) side of the portal - it might be in the wrong zone!
return PortalIntersectResult.INTERSECT_BACK_NO_CROSS;
}
}
// no intersection CURRENTLY. (there might have been an intersection
// during the time between last frame and this frame, but it wasn't a portal
// crossing, and it isn't touching anymore, so it doesn't matter.
return PortalIntersectResult.NO_INTERSECT;
}
else if ( mType == PORTAL_TYPE.PORTAL_TYPE_AABB )
{
// for aabb's we check if the center point went from being inside to being outside
// the aabb (or vice versa) for crossing.
AxisAlignedBox aabb = new AxisAlignedBox( mDerivedCorners[ 0 ], mDerivedCorners[ 1 ] );
//bool previousInside = aabb.contains(pczsn->getPrevPosition());
bool currentInside = aabb.Contains( pczsn.DerivedPosition );
if ( mDirection == Vector3.UnitZ )
{
// portal norm is "outward" pointing, look for going from outside to inside
//if (previousInside == false &&
if ( currentInside == true )
{
return PortalIntersectResult.INTERSECT_CROSS;
}
}
else
{
// portal norm is "inward" pointing, look for going from inside to outside
//if (previousInside == true &&
if ( currentInside == false )
{
return PortalIntersectResult.INTERSECT_CROSS;
}
}
// doesn't cross, but might be touching. This is a little tricky because we only
// care if the node aab is NOT fully contained in the portal aabb because we consider
// the surface of the portal aabb the actual 'portal'. First, check to see if the
// aab of the node intersects the aabb portal
if ( aabb.Intersects( pczsn.WorldAABB ) )
{
// now check if the intersection between the two is not the same as the
// full node aabb, if so, then this means that the node is not fully "contained"
// which is what we are looking for.
AxisAlignedBox overlap = aabb.Intersection( pczsn.WorldAABB );
if ( overlap != pczsn.WorldAABB )
{
return PortalIntersectResult.INTERSECT_NO_CROSS;
}
}
return PortalIntersectResult.NO_INTERSECT;
}
else
{
// for spheres we check if the center point went from being inside to being outside
// the sphere surface (or vice versa) for crossing.
//Real previousDistance2 = mPrevDerivedCP.squaredDistance(pczsn->getPrevPosition());
Real currentDistance2 = mDerivedCP.DistanceSquared( pczsn.DerivedPosition );
Real mRadius2 = mRadius * mRadius;
if ( mDirection == Vector3.UnitZ )
{
// portal norm is "outward" pointing, look for going from outside to inside
//if (previousDistance2 >= mRadius2 &&
if ( currentDistance2 < mRadius2 )
{
return PortalIntersectResult.INTERSECT_CROSS;
}
}
else
{
// portal norm is "inward" pointing, look for going from inside to outside
//if (previousDistance2 < mRadius2 &&
if ( currentDistance2 >= mRadius2 )
{
return PortalIntersectResult.INTERSECT_CROSS;
}
}
// no crossing, but might be touching - check distance
if ( System.Math.Sqrt( System.Math.Abs( mRadius2 - currentDistance2 ) ) <= mRadius )
{
return PortalIntersectResult.INTERSECT_NO_CROSS;
}
return PortalIntersectResult.NO_INTERSECT;
}
}
return PortalIntersectResult.NO_INTERSECT;
}
//----------------------------------------------------------------------------
public Capsule( Segment segment, float radius )
{
this.mSegment = segment;
this.mRadius = radius;
}
