/// <summary>
/// Gets the bounding box of an element in the data.
/// </summary>
/// <param name="triangleIndex">Index of the triangle in the data.</param>
/// <param name="boundingBox">Bounding box of the triangle.</param>
public void GetBoundingBox(int triangleIndex, out BoundingBox boundingBox)
{
System.Numerics.Vector3 v1, v2, v3;
GetTriangle(triangleIndex, out v1, out v2, out v3);
Vector3Ex.Min(ref v1, ref v2, out boundingBox.Min);
Vector3Ex.Min(ref boundingBox.Min, ref v3, out boundingBox.Min);
Vector3Ex.Max(ref v1, ref v2, out boundingBox.Max);
Vector3Ex.Max(ref boundingBox.Max, ref v3, out boundingBox.Max);
}
/// <summary>
/// Initializes the detector entity and any other necessary logic.
/// </summary>
protected internal override void Initialize()
{
//Setup the dimensions of the detector.
System.Numerics.Vector3 startpoint = wheel.suspension.localAttachmentPoint;
System.Numerics.Vector3 endpoint = startpoint + wheel.suspension.localDirection * wheel.suspension.restLength;
System.Numerics.Vector3 min, max;
Vector3Ex.Min(ref startpoint, ref endpoint, out min);
Vector3Ex.Max(ref startpoint, ref endpoint, out max);
detector.Width = max.X - min.X;
detector.Height = max.Y - min.Y;
detector.Length = max.Z - min.Z;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
System.Numerics.Vector3 a, b, c;
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
Matrix3x3.Transform(ref vA, ref o, out a);
Matrix3x3.Transform(ref vB, ref o, out b);
Matrix3x3.Transform(ref vC, ref o, out c);
Vector3Ex.Min(ref a, ref b, out boundingBox.Min);
Vector3Ex.Min(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3Ex.Max(ref a, ref b, out boundingBox.Max);
Vector3Ex.Max(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
protected internal override void SolveVelocityIteration()
{
//Compute the 'relative' linear and angular velocities. For single bone constraints, it's based entirely on the one bone's velocities!
//They have to be pulled into constraint space first to compute the necessary impulse, though.
System.Numerics.Vector3 linearContributionA;
Matrix3x3.TransformTranspose(ref ConnectionA.linearVelocity, ref linearJacobianA, out linearContributionA);
System.Numerics.Vector3 angularContributionA;
Matrix3x3.TransformTranspose(ref ConnectionA.angularVelocity, ref angularJacobianA, out angularContributionA);
System.Numerics.Vector3 linearContributionB;
Matrix3x3.TransformTranspose(ref ConnectionB.linearVelocity, ref linearJacobianB, out linearContributionB);
System.Numerics.Vector3 angularContributionB;
Matrix3x3.TransformTranspose(ref ConnectionB.angularVelocity, ref angularJacobianB, out angularContributionB);
//The constraint velocity error will be the velocity we try to remove.
System.Numerics.Vector3 constraintVelocityError;
Vector3Ex.Add(ref linearContributionA, ref angularContributionA, out constraintVelocityError);
Vector3Ex.Add(ref constraintVelocityError, ref linearContributionB, out constraintVelocityError);
Vector3Ex.Add(ref constraintVelocityError, ref angularContributionB, out constraintVelocityError);
//However, we need to take into account two extra sources of velocities which modify our target velocity away from zero.
//First, the velocity bias from position correction:
Vector3Ex.Subtract(ref constraintVelocityError, ref velocityBias, out constraintVelocityError);
//And second, the bias from softness:
System.Numerics.Vector3 softnessBias;
Vector3Ex.Multiply(ref accumulatedImpulse, -softness, out softnessBias);
Vector3Ex.Subtract(ref constraintVelocityError, ref softnessBias, out constraintVelocityError);
//By now, the constraint velocity error contains all the velocity we want to get rid of.
//Convert it into an impulse using the effective mass matrix.
System.Numerics.Vector3 constraintSpaceImpulse;
Matrix3x3.Transform(ref constraintVelocityError, ref effectiveMass, out constraintSpaceImpulse);
Vector3Ex.Negate(ref constraintSpaceImpulse, out constraintSpaceImpulse);
//Add the constraint space impulse to the accumulated impulse so that warm starting and softness work properly.
System.Numerics.Vector3 preadd = accumulatedImpulse;
Vector3Ex.Add(ref constraintSpaceImpulse, ref accumulatedImpulse, out accumulatedImpulse);
//Limits can only apply positive impulses.
Vector3Ex.Max(ref Toolbox.ZeroVector, ref accumulatedImpulse, out accumulatedImpulse);
//But wait! The accumulated impulse may exceed this constraint's capacity! Check to make sure!
float impulseSquared = accumulatedImpulse.LengthSquared();
if (impulseSquared > maximumImpulseSquared)
{
//Oops! Clamp that down.
Vector3Ex.Multiply(ref accumulatedImpulse, maximumImpulse / (float)Math.Sqrt(impulseSquared), out accumulatedImpulse);
}
//Update the impulse based upon the clamped accumulated impulse and the original, pre-add accumulated impulse.
Vector3Ex.Subtract(ref accumulatedImpulse, ref preadd, out constraintSpaceImpulse);
//The constraint space impulse now represents the impulse we want to apply to the bone... but in constraint space.
//Bring it out to world space using the transposed jacobian.
if (!ConnectionA.Pinned)//Treat pinned elements as if they have infinite inertia.
{
System.Numerics.Vector3 linearImpulseA;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref linearJacobianA, out linearImpulseA);
System.Numerics.Vector3 angularImpulseA;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref angularJacobianA, out angularImpulseA);
//Apply them!
ConnectionA.ApplyLinearImpulse(ref linearImpulseA);
ConnectionA.ApplyAngularImpulse(ref angularImpulseA);
}
if (!ConnectionB.Pinned)//Treat pinned elements as if they have infinite inertia.
{
System.Numerics.Vector3 linearImpulseB;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref linearJacobianB, out linearImpulseB);
System.Numerics.Vector3 angularImpulseB;
Matrix3x3.Transform(ref constraintSpaceImpulse, ref angularJacobianB, out angularImpulseB);
//Apply them!
ConnectionB.ApplyLinearImpulse(ref linearImpulseB);
ConnectionB.ApplyAngularImpulse(ref angularImpulseB);
}
}
internal void Attack(BaseCombatEntity ent, Act active = Act.Attack)
{
targetentity = ent;
action = active;
attackrange = Math.Pow(Vector3Ex.Max(BoundsExtension.XZ3D(Base._collider.bounds).extents) + Base.attack.range + Vector3Ex.Max(BoundsExtension.XZ3D(ent._collider.bounds).extents), 2);
}
