/**
* Initialize the graphics and objects needed to run the program
*/
void Start()
{
stepLabel = GameObject.Find("StepLabel").GetComponent <Text> ();
moved = new Queue <Polygon> ();
done = new HashSet <Polygon> ();
closestPts = new Dictionary <Tuple <int, int>, GameObject> ();
polys = new List <Polygon> ();
GameObject a = new GameObject("GJK");
gjk = a.AddComponent <GJK> () as GJK;
GameObject b = new GameObject("Inputs");
im = b.AddComponent <InputManager> () as InputManager;
im.sm = this;
newPolypoints = new List <Vector2>();
GameObject newPoly = new GameObject();
newPolyLine = newPoly.AddComponent <LineRenderer> ();
newPolyLine.material = new Material(Shader.Find("Particles/Additive"));
newPolyLine.widthMultiplier = 0.03f;
newPolyLine.numPositions = 0;
GameObject minkiDinki = new GameObject("Minkow");
minkiDinkiLine = minkiDinki.AddComponent <LineRenderer> ();
minkiDinkiLine.material = new Material(Shader.Find("Particles/Additive"));
minkiDinkiLine.widthMultiplier = 0.03f;
minkiDinkiLine.numPositions = 0;
}
public ContactPoint[] IsColliding(Transform at, Transform bt, ConvexShape other)
{
float depth = GJK.DoGJK(this, other, at, bt, out var normal, out var pos);
if (depth < 0)
{
return(new ContactPoint[] { });
}
else
{
float friction = this.frictionFactor * other.frictionFactor;
bool nofriction = Math.Abs(friction) < float.Epsilon;
ContactPoint[] cps;
if (nofriction)
{
cps = new ContactPoint[1];
}
else
{
cps = new ContactPoint[3];
}
ContactPoint cp = new ContactPoint
{
position = pos,
normal = normal,
depth = depth,
bodyA = this.root,
bodyB = other.root
};
cps[0] = cp;//Normal contact
if (!nofriction)
{
Vector3 frictionVecA = Vector3.Normalize(Vector3.Cross(cp.normal, cp.normal + Vector3.UnitX));
Vector3 frictionVecB = Vector3.Normalize(Vector3.Cross(cp.normal, frictionVecA));
cp.normal = frictionVecA * friction;
cp.depth = 0;
cps[1] = cp;
cp.normal = frictionVecB * friction;
cps[2] = cp;
}
return(cps);
}
}
// Update is called once per frame
private void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
if (_state == null)
{
_state = new GJKState(ref PolytopeA, ref PolytopeB);
_state.LastDirection = initDirection;
}
_minkowskisumPoints.Clear();
_minkowskisumPoints = MinkowskiSum.CalcMinkowskiSum(PolytopeA, PolytopeB);
_gjkSupportPoints.Clear();
if (!_state.FinishRun)
{
GJK.Collided(ref _state);
for (int i = 0; i < _state.CurrentSimplex.GetSize(); i++)
{
_gjkSupportPoints.Add(_state.CurrentSimplex.PeekAt(i));
}
}
else
{
// Physics res
float c = 0.5f;
PhysicsSim compA = _state.GetPolytopeA.GetComponent <PhysicsSim>();
PhysicsSim compB = _state.GetPolytopeB.GetComponent <PhysicsSim>();
compA.velocity = UIController.initVelocutyA;
compB.velocity = UIController.initVelocutyB;
float massTotal = compA.mass + compB.mass;
// Contacts
Vector3 lpA = _state.CurrentSimplex.PopBack();
Vector3 lpB = _state.CurrentSimplex.PopBack();
Vector3 cpA = _state.epaData.Contact;
Vector3 cpB = cpA;
// Inert
float invVal = 1f / 0.16f;
Matrix4x4 rotInert = Matrix4x4.identity;
rotInert.SetRow(0, new Vector4(invVal, 0f, 0f, 0f));
rotInert.SetRow(1, new Vector4(0f, invVal, 0f, 0f));
rotInert.SetRow(2, new Vector4(0f, 0f, 0f, invVal));
float j =
(-(1f + c) * Vector3.Dot(compA.velocity - compB.velocity, _state.epaData.Normal)) /
((1f / compA.mass + 1f / compB.mass) +
Vector3.Dot(
Vector3.Cross(rotInert * Vector3.Cross(cpA, _state.epaData.Normal), cpA) +
Vector3.Cross(rotInert * Vector3.Cross(cpB, _state.epaData.Normal), cpB),
_state.epaData.Normal
));
Vector4 wa = rotInert *
(
Vector3.Cross(
cpA,
j * _state.epaData.Normal
)
);
compA.AngularVelocity.x += wa.x;
compA.AngularVelocity.y += wa.y;
compA.AngularVelocity.z += wa.z;
Vector4 wb = rotInert *
(
Vector3.Cross(
cpB,
j * _state.epaData.Normal
)
);
compB.AngularVelocity.x -= wb.x;
compB.AngularVelocity.y -= wb.y;
compB.AngularVelocity.z -= wb.z;
compA.velocity = compA.velocity + ((j / compA.mass) * _state.epaData.Normal);
compB.velocity = compA.velocity - ((j / compB.mass) * _state.epaData.Normal);
_state = null;
}
}
}
//
public static bool Penetration( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results,
bool usemargins = false )
{
tShape shape = new tShape();
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins );
GJK gjk = new GJK();
btVector3 tmp;
guess.Invert( out tmp );
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp );
switch( gjk_status )
{
case GJK.eStatus._.Inside:
{
EPA epa = new EPA();
EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp );
if( epa_status != EPA.eStatus._.Failed )
{
btVector3 w0 = btVector3.Zero;
for( uint i = 0; i < epa.m_result.rank; ++i )
{
shape.Support( ref epa.m_result.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, epa.m_result.p[i], out w0 );
}
results.status = sResults.eStatus.Penetrating;
wtrs0.Apply( ref w0, out results.witness0 );
w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp );
wtrs0.Apply( ref tmp, out results.witness1 );
epa.m_normal.Invert( out results.normal );
results.distance = -epa.m_depth;
return ( true );
}
else results.status = sResults.eStatus.EPA_Failed;
}
break;
case GJK.eStatus._.Failed:
results.status = sResults.eStatus.GJK_Failed;
break;
default:
break;
}
return ( false );
}
//
// Api
//
//
//
public static bool Distance( btConvexShape shape0,
ref btTransform wtrs0,
btConvexShape shape1,
ref btTransform wtrs1,
ref btVector3 guess,
out sResults results )
{
tShape shape = new tShape();
results.witness0 =
results.witness1 = btVector3.Zero;
results.status = btGjkEpaSolver2.sResults.eStatus.Separated;
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref guess );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
gjk.m_simplex.c[i].d.Invert( out tmp );
shape.Support( ref tmp, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
w0.Sub( ref w1, out results.normal );
results.distance = results.normal.length();
results.normal.Div( ( results.distance > GJK_MIN_DISTANCE ) ? results.distance : 1, out results.normal );
return ( true );
}
else
{
results.status = gjk_status == GJK.eStatus._.Inside
? sResults.eStatus.Penetrating
: sResults.eStatus.GJK_Failed;
return ( false );
}
}
internal eStatus._ Evaluate( GJK gjk, ref btVector3 guess )
{
GJK.sSimplex simplex = gjk.m_simplex;
if( ( simplex.rank > 1 ) && gjk.EncloseOrigin() )
{
/* Clean up */
while( m_hull.root != null )
{
sFace f = m_hull.root;
remove( m_hull, f );
append( m_stock, f );
}
m_status = eStatus._.Valid;
m_nextsv = 0;
/* Orient simplex */
if( btVector3.det( ref simplex.c[0].w, ref simplex.c[3].w,
ref simplex.c[1].w, ref simplex.c[3].w,
ref simplex.c[2].w, ref simplex.c[3].w ) < 0 )
{
btScalar.btSwap( ref simplex.c[0], ref simplex.c[1] );
btScalar.btSwap( ref simplex.p[0], ref simplex.p[1] );
}
/* Build initial hull */
sFace[] tetra = {newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
if( m_hull.count == 4 )
{
sFace best = findbest();
sFace outer = new sFace( best );
uint pass = 0;
uint iterations = 0;
bind( tetra[0], 0, tetra[1], 0 );
bind( tetra[0], 1, tetra[2], 0 );
bind( tetra[0], 2, tetra[3], 0 );
bind( tetra[1], 1, tetra[3], 2 );
bind( tetra[1], 2, tetra[2], 1 );
bind( tetra[2], 2, tetra[3], 1 );
m_status = eStatus._.Valid;
for( ; iterations < EPA_MAX_ITERATIONS; ++iterations )
{
if( m_nextsv < EPA_MAX_VERTICES )
{
sHorizon horizon = new sHorizon();
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (byte)( ++pass );
gjk.getsupport( ref best.n, w );
double wdist = best.n.dot( ref w.w ) - best.d;
if( wdist > EPA_ACCURACY )
{
for( uint j = 0; ( j < 3 ) && valid; ++j )
{
valid &= expand( pass, w,
best.f[j], best.e[j],
horizon );
}
if( valid & ( horizon.nf >= 3 ) )
{
bind( horizon.cf, 1, horizon.ff, 2 );
remove( m_hull, best );
append( m_stock, best );
best = findbest();
outer = new sFace( best );
}
else { m_status = eStatus._.InvalidHull; break; }
}
else { m_status = eStatus._.AccuraryReached; break; }
}
else { m_status = eStatus._.OutOfVertices; break; }
}
btVector3 projection; outer.n.Mult( outer.d, out projection );
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
btVector3 tmp;
btVector3.btCross2Del( ref outer.c[1].w, ref projection,
ref outer.c[2].w, ref projection, out tmp );
m_result.p[0] = tmp.length();
btVector3.btCross2Del( ref outer.c[2].w, ref projection,
ref outer.c[0].w, ref projection, out tmp );
m_result.p[1] = tmp.length();
btVector3.btCross2Del( ref outer.c[0].w, ref projection,
ref outer.c[1].w, ref projection, out tmp );
m_result.p[2] = tmp.length();
double sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return ( m_status );
}
}
/* Fallback */
m_status = eStatus._.FallBack;
guess.Invert( out m_normal );
//m_normal = -guess;
double nl = m_normal.length();
if( nl > 0 )
m_normal.Div( nl, out m_normal );
else
m_normal = btVector3.xAxis;
m_depth = 0;
m_result.rank = 1;
m_result.c[0] = simplex.c[0];
m_result.p[0] = 1;
return ( m_status );
}
//
public static double SignedDistance( ref btVector3 position,
double margin,
btConvexShape shape0,
ref btTransform wtrs0,
sResults results )
{
tShape shape = new tShape();
using( btSphereShape shape1 = BulletGlobals.SphereShapePool.Get() )
{
shape1.Initialize( margin );
btTransform wtrs1 = new btTransform( ref btQuaternion.Zero, ref position );
Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false );
GJK gjk = new GJK();
GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref btVector3.One );
if( gjk_status == GJK.eStatus._.Valid )
{
btVector3 w0 = btVector3.Zero;
btVector3 w1 = btVector3.Zero;
for( uint i = 0; i < gjk.m_simplex.rank; ++i )
{
double p = gjk.m_simplex.p[i];
btVector3 tmp;
shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp );
w0.AddScale( ref tmp, p, out w0 );
btVector3 tmp2;
gjk.m_simplex.c[i].d.Invert( out tmp2 );
shape.Support( ref tmp2, 1, out tmp );
w1.AddScale( ref tmp, p, out w1 );
}
wtrs0.Apply( ref w0, out results.witness0 );
wtrs0.Apply( ref w1, out results.witness1 );
btVector3 delta; results.witness1.Sub( ref results.witness0, out delta );
margin = shape0.getMarginNonVirtual() +
shape1.getMarginNonVirtual();
double length = delta.length();
delta.Div( length, out results.normal );
results.witness0.AddScale( ref results.normal, margin, out results.witness0 );
return ( length - margin );
}
else
{
if( gjk_status == GJK.eStatus._.Inside )
{
if( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, out results ) )
{
btVector3 delta; results.witness0.Sub(
ref results.witness1, out delta );
double length = delta.length();
if( length >= btScalar.SIMD_EPSILON )
delta.Div( length, out results.normal );
return ( -length );
}
}
}
}
return ( btScalar.SIMD_INFINITY );
}
