public static txColliderState DiskLineSegment(txPhysicalShpere d_, txLineSegment line, ref txColliderPatch patch)
{
txVector2 a = d_.Position - line.start;
txVector2 b = line.end - line.start;
// debug
//double ab = a * b;
//txVector2 bnormal = b.Normalize();
txVector2 disVec = a - a * b * (b.Normalize()) * (1 / b.Length());
txVector2 normal = disVec.Normalize();
double distance = disVec.Length();
txOrientationState orientationstate = txVector2.PointOrientationTest(line.start, line.end, d_.Position);
if (txOrientationState.LEFT == orientationstate)
{
if (distance < txPhysicalShpere.RADIUS)
{
patch.penetrationdepth = 0.5 * (txPhysicalShpere.RADIUS - distance);
patch.normal = normal;
return txColliderState.PENETRATION;
}
if (distance == txPhysicalShpere.RADIUS)
{
patch.penetrationdepth = 0.0;
patch.normal = normal;
return txColliderState.CONTACT;
}
// >
return txColliderState.SEPERATE;
}
if (txOrientationState.RIGHT == orientationstate)
{
patch.penetrationdepth = distance + txPhysicalShpere.RADIUS;
patch.normal = -1.0 * normal;
return txColliderState.PENETRATION;
}
// If I embed the following code into the comment code
// The compiler will issue a error
// not all code paths return a value????
//if (txOrientationState.COLLINEAR == orientationstate)
//{
txVector2 verticalv;
verticalv.x = b.y;
verticalv.y = -b.x;
patch.penetrationdepth = txPhysicalShpere.RADIUS;
patch.normal = verticalv.Normalize();
return txColliderState.PENETRATION;
//}
}
// Calcualte the I's patch
// In later versiont the J will be calculate at the same time to be more efficiency!
public static txColliderState DiskDisk(txPhysicalShpere I, txPhysicalShpere J, ref txColliderPatch patch)
{
double squaredistance = I.SquareDistanceToShpere(J);
if (squaredistance < txPhysicalShpere.RADIUS4SQUARE) {
patch.normal = (J.Position - I.Position).Normalize();
patch.penetrationdepth = 0.25 * (txPhysicalShpere.DIAMETER - Math.Sqrt(squaredistance));
return txColliderState.PENETRATION;
}
else if (squaredistance > txPhysicalShpere.RADIUS4SQUARE)
{
return txColliderState.SEPERATE;
}
else {
return txColliderState.CONTACT;
};
}
public double SquareDistanceToShpere(txPhysicalShpere s_)
{
return position.SquareDistance(s_.position);
}
private static txColliderPatch AddNewPatchToDisk(txPhysicalShpere d_, txColliderPatch currentpatch, txColliderState collstate)
{
if (txColliderState.PENETRATION == collstate)
{
d_.CollisionPatches.Add(currentpatch);
}
else if (txColliderState.SEPERATE == collstate)
{
}
else
{
}
return currentpatch;
}
public static void DiskToLineSegmentList(txPhysicalShpere d_, List<txLineSegment> llist)
{
txColliderPatch currentpatch = new txColliderPatch();
foreach (txLineSegment it in llist)
{
txColliderState collisionstate = DiskLineSegment(d_, it,ref currentpatch);
AddNewPatchToDisk(d_,currentpatch,collisionstate);
}
}
public static void DiskToDiskList(txPhysicalShpere d_, List<txPhysicalShpere> dlist_)
{
txColliderPatch currentpatch = new txColliderPatch();
foreach (txPhysicalShpere it in dlist_)
{
if (it != d_)
{
txColliderState collstate = DiskDisk(it, d_, ref currentpatch);
currentpatch = AddNewPatchToDisk(d_, currentpatch, collstate);
}
}
}
private void SetUpDisk1010()
{
// set up disklist
const double xstart = -132.0;
const double ystart = -132.0;
double x = xstart;
double y = ystart;
const double deltax = 21.0;
const double deltay = 21.0;
for (int i = 0; i < 10; i++)
{
x = xstart;
for (int j = 0; j < 10; j++)
{
txPhysicalShpere disk = new txPhysicalShpere();
txVector2 position;
position.x = x;
position.y = y;
disk.Position = position;
disk.Velocity = txVector2.Zero();
disk.Acceleration = txVector2.Zero();
disk.Force = txVector2.Zero();
disk.Stiffness = stiffnessparameter;
disk.Id = -1;
disk.CollisionPatches.Clear();
disklist.Add(disk);
x += deltax;
}
y += deltay;
}
}
