public override void OnUpdate()
{
base.OnUpdate();
rotation += FrameDelta;
rotBody.CenterOfMassTransform = Matrix.RotationX(rotation) * rotBodyPosition;
var input = new DiscreteCollisionDetectorInterface.ClosestPointInput
{
TransformA = rotBody.CenterOfMassTransform,
TransformB = body2Position
};
var result = new PointCollector();
using (var detector = new GjkPairDetector(colShape0, colShape1, sGjkSimplexSolver, null))
{
detector.CachedSeparatingAxis = new Vector3(0.00000000f, 0.059727669f, 0.29259586f);
detector.GetClosestPoints(input, result, null);
}
if (result.HasResult)
{
distanceFrom = result.PointInWorld;
distanceTo = result.PointInWorld + result.NormalOnBInWorld * result.Distance;
distance = result.Distance;
World.DebugDrawer.DrawLine(ref distanceFrom, ref distanceTo, ref red);
}
result.Dispose();
}
//static Vector3 red = new Vector3(1.0f, 0.0f, 0.0f);
public override void OnUpdate()
{
base.OnUpdate();
rotation += FrameDelta;
rotBody.CenterOfMassTransform = Matrix.RotationX(rotation) * rotBodyPosition;
GjkPairDetector detector = new GjkPairDetector(colShape0, colShape1, sGjkSimplexSolver, null);
detector.CachedSeparatingAxis = new Vector3(0.00000000f, 0.059727669f, 0.29259586f);
GjkPairDetector.ClosestPointInput input = new GjkPairDetector.ClosestPointInput();
input.TransformA = rotBody.CenterOfMassTransform;
input.TransformB = body2Position;
PointCollector result = new PointCollector();
detector.GetClosestPoints(input, result, null);
/*
if (result.HasResult && IsDebugDrawEnabled)
{
distanceFrom = result.PointInWorld;
distanceTo = result.PointInWorld + result.NormalOnBInWorld * result.Distance;
distance = result.Distance;
//World.DebugDrawer.DrawLine(ref distanceFrom, ref distanceTo, ref red);
}
*/
}
public override int Update(float elapsedTime)
{
int subSteps = base.Update(elapsedTime);
rotation += elapsedTime;
rotBody.CenterOfMassTransform = Matrix.RotationX(rotation) * rotBodyPosition;
GjkPairDetector detector = new GjkPairDetector(colShape0, colShape1, sGjkSimplexSolver, null);
detector.CachedSeparatingAxis = new Vector3(0.00000000f, 0.059727669f, 0.29259586f);
GjkPairDetector.ClosestPointInput input = new GjkPairDetector.ClosestPointInput();
input.TransformA = rotBody.CenterOfMassTransform;
input.TransformB = body2Position;
PointCollector result = new PointCollector();
detector.GetClosestPoints(input, result, null);
if (result.HasResult)
{
HasDistanceResult = true;
distanceFrom = result.PointInWorld;
distanceTo = result.PointInWorld + result.NormalOnBInWorld * result.Distance;
distance = result.Distance;
}
else
{
HasDistanceResult = false;
}
return subSteps;
}
public unsafe static void GetNormalOnBInWorld(this PointCollector obj, out OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
*(BulletSharp.Math.Vector3 *)valuePtr = obj.NormalOnBInWorld;
}
}
public unsafe static void SetPointInWorld(this PointCollector obj, ref OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
obj.PointInWorld = *(BulletSharp.Math.Vector3 *)valuePtr;
}
}
/// <summary>
/// 2つのコリジョン オブジェクトの距離
/// </summary>
/// <param name="colA">コリジョンA</param>
/// <param name="colB">コリジョンB</param>
/// <returns></returns>
public static float Distance(CollisionObject colA, CollisionObject colB)
{
if (colA == null || colA.Shape == null || colB == null || colB.Shape == null ||
colA.Data == null || colB.Data == null) {
// 距離が定義できない場合は
// NaNを返す
return Single.NaN;
}
var shpA = colA.Data.CollisionShape as ConvexShape;
var shpB = colB.Data.CollisionShape as ConvexShape;
var solver = new VoronoiSimplexSolver ();
var detector = new GjkPairDetector (shpA, shpB, solver, null);
var traA = Matrix4x4.CreateFromTranslation (colA.Offset) * colA.Node.GlobalTransform;
var traB = Matrix4x4.CreateFromTranslation (colB.Offset) * colB.Node.GlobalTransform;
var input = new GjkPairDetector.ClosestPointInput ();
input.TransformA = traA.ToBullet ();
input.TransformB = traB.ToBullet ();
input.MaximumDistanceSquared = Single.MaxValue;
var output = new PointCollector ();
detector.GetClosestPoints (input, output, null);
if (!output.HasResult) {
// 重なっていた場合は
// 0を返す
return 0;
}
// 正常な距離は0以上
return output.Distance;
}
public static void SetPointInWorld(this PointCollector obj, OpenTK.Vector3 value)
{
SetPointInWorld(obj, ref value);
}
public static OpenTK.Vector3 GetPointInWorld(this PointCollector obj)
{
OpenTK.Vector3 value;
GetPointInWorld(obj, out value);
return(value);
}