public override void Redraw(float currentTime, float elapsedTime)
{
// selected vehicle (user can mouse click to select another)
IVehicle selected = Demo.SelectedVehicle;
// vehicle nearest mouse (to be highlighted)
IVehicle nearMouse = Demo.VehicleNearestToMouse();
// update camera
Demo.UpdateCamera(elapsedTime, selected);
// draw "ground plane"
Demo.GridUtility(selected.Position);
// update, draw and annotate each agent
foreach (LowSpeedTurn agent in _all)
{
agent.Draw();
// display speed near agent's screen position
Color textColor = new Color(new Vector3(0.8f, 0.8f, 1.0f).ToXna());
Vector3 textOffset = new Vector3(0, 0.25f, 0);
Vector3 textPosition = agent.Position + textOffset;
String annote = String.Format("{0:0.00}", agent.Speed);
Drawing.Draw2dTextAt3dLocation(annote, textPosition, textColor);
}
// highlight vehicle nearest mouse
Demo.HighlightVehicleUtility(nearMouse);
}
// get a value based on a position in 3d world space
private bool GetMapValue(Vector3 point)
{
Vector3 local = point - Center;
local.Y = 0;
Vector3 localXZ = local;
float hxs = XSize / 2;
float hzs = ZSize / 2;
float x = localXZ.X;
float z = localXZ.Z;
bool isOut = (x > +hxs) || (x < -hxs) || (z > +hzs) || (z < -hzs);
if (isOut)
{
return _outsideValue;
}
else
{
int i = (int)Utilities.RemapInterval(x, -hxs, hxs, 0.0f, Resolution);
int j = (int)Utilities.RemapInterval(z, -hzs, hzs, 0.0f, Resolution);
return GetMapBit(i, j);
}
}
// ------------------------------------------------------------------------
// drawing of lines, circles and (filled) disks to annotate steering
// behaviors. When called during OpenSteerDemo's simulation update phase,
// these functions call a "deferred draw" routine which buffer the
// arguments for use during the redraw phase.
//
// note: "circle" means unfilled
// "disk" means filled
// "XZ" means on a plane parallel to the X and Z axes (perp to Y)
// "3d" means the circle is perpendicular to the given "axis"
// "segments" is the number of line segments used to draw the circle
// draw an opaque colored line segment between two locations in space
public void Line(Vector3 startPoint, Vector3 endPoint, Vector3 color, float opacity = 1)
{
if (_isEnabled && Drawer != null)
{
Drawer.Line(startPoint, endPoint, new Color(new Microsoft.Xna.Framework.Vector3(color.X, color.Y, color.Z)), opacity);
}
}
/// <summary>
/// Cleans up the pair tester.
/// </summary>
public override void CleanUp()
{
convex = null;
state = CollisionState.Plane;
escapeAttempts = 0;
localSeparatingAxis = new System.Numerics.Vector3();
Updated = false;
}
///<summary>
/// Sets up the contact with new information.
///</summary>
///<param name="candidate">Contact data to initialize the contact with.</param>
public void Setup(ref ContactData candidate)
{
candidate.Validate();
Position = candidate.Position;
Normal = candidate.Normal;
PenetrationDepth = candidate.PenetrationDepth;
Id = candidate.Id;
}
///<summary>
/// Constructs a new affine transform.
///</summary>
///<param name="scaling">Scaling to apply in the linear transform.</param>
///<param name="orientation">Orientation to apply in the linear transform.</param>
///<param name="translation">Translation to apply.</param>
public AffineTransform(System.Numerics.Vector3 scaling, System.Numerics.Quaternion orientation, System.Numerics.Vector3 translation)
{
//Create an SRT transform.
Matrix3x3.CreateScale(ref scaling, out LinearTransform);
Matrix3x3 rotation;
Matrix3x3.CreateFromQuaternion(ref orientation, out rotation);
Matrix3x3.Multiply(ref LinearTransform, ref rotation, out LinearTransform);
Translation = translation;
}
///<summary>
/// Constructs a triangle shape from cached data.
///</summary>
///<param name="vA">First vertex in the triangle.</param>
///<param name="vB">Second vertex in the triangle.</param>
///<param name="vC">Third vertex in the triangle.</param>
/// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param>
public TriangleShape(System.Numerics.Vector3 vA, System.Numerics.Vector3 vB, System.Numerics.Vector3 vC, ConvexShapeDescription description)
{
//Recenter. Convexes should contain the origin.
var center = (vA + vB + vC) / 3;
this.vA = vA - center;
this.vB = vB - center;
this.vC = vC - center;
UpdateConvexShapeInfo(description);
}
///<summary>
/// Cleans up the pair tester.
///</summary>
public void CleanUp()
{
state = CollisionState.Separated;
previousState = CollisionState.Separated;
cachedSimplex = new CachedSimplex();
localSeparatingAxis = new System.Numerics.Vector3();
collidableA = null;
collidableB = null;
}
// reset state
public override void Reset()
{
base.Reset(); // reset the vehicle
Speed = 0.0f; // speed along Forward direction.
Position = new Vector3(0, 0, 0);
if (_trail == null) _trail = new Trail(100, 6000);
_trail.Clear(); // prevent long streaks due to teleportation
}
///<summary>
/// Constructs a triangle shape.
/// The vertices will be recentered.
///</summary>
///<param name="vA">First vertex in the triangle.</param>
///<param name="vB">Second vertex in the triangle.</param>
///<param name="vC">Third vertex in the triangle.</param>
///<param name="center">Computed center of the triangle.</param>
public TriangleShape(System.Numerics.Vector3 vA, System.Numerics.Vector3 vB, System.Numerics.Vector3 vC, out System.Numerics.Vector3 center)
{
//Recenter. Convexes should contain the origin.
center = (vA + vB + vC) / 3;
this.vA = vA - center;
this.vB = vB - center;
this.vC = vC - center;
UpdateConvexShapeInfo(ComputeDescription(vA, vB, vC, collisionMargin));
}
private void CreateDatabase()
{
Vector3 center = Vector3.Zero;
const float DIV = 10.0f;
Vector3 divisions = new Vector3(DIV, DIV, DIV);
const float DIAMETER = Fighter.WORLD_RADIUS * 2;
Vector3 dimensions = new Vector3(DIAMETER, DIAMETER, DIAMETER);
_pd = new LocalityQueryProximityDatabase<IVehicle>(center, dimensions, divisions);
}
public void Draw()
{
Vector3 b = new Vector3(_min.X, 0, _max.Z);
Vector3 c = new Vector3(_max.X, 0, _min.Z);
Color color = new Color(255, 255, 0);
Drawing.DrawLineAlpha(_min, b, color, 1.0f);
Drawing.DrawLineAlpha(b, _max, color, 1.0f);
Drawing.DrawLineAlpha(_max, c, color, 1.0f);
Drawing.DrawLineAlpha(c, _min, color, 1.0f);
}
public void DrawHomeBase()
{
Vector3 up = new Vector3(0, 0.01f, 0);
Color atColor = new Color((byte)(255.0f * 0.3f), (byte)(255.0f * 0.3f), (byte)(255.0f * 0.5f));
Color noColor = Color.Gray;
bool reached = Plugin.CtfSeeker.State == SeekerState.AtGoal;
Color baseColor = (reached ? atColor : noColor);
Drawing.DrawXZDisk(_baseRadius, Globals.HomeBaseCenter, baseColor, 40);
Drawing.DrawXZDisk(_baseRadius / 15, Globals.HomeBaseCenter + up, Color.Black, 20);
}
public void AnnotateAvoidNeighbor(IVehicle threat, Vector3 ourFuture, Vector3 threatFuture)
{
Color green = new Color((byte)(255.0f * 0.15f), (byte)(255.0f * 0.6f), 0);
annotation.Line(Position, ourFuture, green.ToVector3().FromXna());
annotation.Line(threat.Position, threatFuture, green.ToVector3().FromXna());
annotation.Line(ourFuture, threatFuture, Color.Red.ToVector3().FromXna());
annotation.CircleXZ(Radius, ourFuture, green.ToVector3().FromXna(), 12);
annotation.CircleXZ(Radius, threatFuture, green.ToVector3().FromXna(), 12);
}
public static void AddToBuffer(Vector3 s, Vector3 e, Color c)
{
if (_index >= _deferredLines.Count)
_deferredLines.Add(new DeferredLine());
_deferredLines[_index]._startPoint = s;
_deferredLines[_index]._endPoint = e;
_deferredLines[_index]._color = c;
_index++;
}
static int Main(string[] args)
{
Point a = new Point(1, 2, 3);
Point b = new Point(2, 2, 5);
float c = 33;
Point d = (b + a) * c;
Point q = d + a;
if (CheckEQ(q.X, 100))
{
return 100;
}
return 0;
}
public static void AddToBuffer(float radius, Vector3 axis, Vector3 center, Color color, int segments, bool filled, bool in3D)
{
if (_index < SIZE)
{
_deferredCircleArray[_index]._radius = radius;
_deferredCircleArray[_index]._axis = axis;
_deferredCircleArray[_index]._center = center;
_deferredCircleArray[_index]._color = color;
_deferredCircleArray[_index]._segments = segments;
_deferredCircleArray[_index]._filled = filled;
_deferredCircleArray[_index]._in3D = in3D;
_index++;
}
}
public TerrainMap(Vector3 c, float x, float z, int r)
{
Center = c;
XSize = x;
ZSize = z;
Resolution = r;
_outsideValue = false;
_map = new bool[Resolution * Resolution];
for (int i = 0; i < Resolution * Resolution; i++)
{
_map[i] = false;
}
}
/// <summary>
/// Records a position for the current time, called once per update.
/// </summary>
/// <param name="currentTime"></param>
/// <param name="position"></param>
public void Record(float currentTime, Vector3 position)
{
float timeSinceLastTrailSample = currentTime - _lastSampleTime;
if (timeSinceLastTrailSample > _sampleInterval)
{
_currentIndex = (_currentIndex + 1) % _vertices.Length;
_vertices[_currentIndex] = position;
_dottedPhase = (_dottedPhase + 1) % 2;
bool tick = (Math.Floor(currentTime) > Math.Floor(_lastSampleTime));
_flags[_currentIndex] = (byte)(_dottedPhase | (tick ? 2 : 0));
_lastSampleTime = currentTime;
}
_currentPosition = position;
}
///<summary>
/// Computes the expansion of the minkowski sum due to margins in a given direction.
///</summary>
///<param name="marginA">First margin.</param>
///<param name="marginB">Second margin.</param>
///<param name="direction">Extreme point direction.</param>
///<param name="contribution">Margin contribution to the extreme point.</param>
public static void ExpandMinkowskiSum(float marginA, float marginB, ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 contribution)
{
float lengthSquared = direction.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
//The contribution to the minkowski sum by the margin is:
//direction * marginA - (-direction) * marginB.
Vector3Ex.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution);
}
else
{
contribution = new System.Numerics.Vector3();
}
}
// reset state
public override void Reset()
{
base.Reset(); // reset the vehicle
Speed = 0.0f; // speed along Forward direction.
// Place me on my part of the field, looking at oponnents goal
Position = new Vector3(_imTeamA ? RandomHelpers.Random() * 20 : -RandomHelpers.Random() * 20, 0, (RandomHelpers.Random() - 0.5f) * 20);
if (_myID < 9)
{
Position = _imTeamA ? (Globals.PlayerPosition[_myID]) : (new Vector3(-Globals.PlayerPosition[_myID].X, Globals.PlayerPosition[_myID].Y, Globals.PlayerPosition[_myID].Z));
}
_home = Position;
if (_trail == null) _trail = new Trail(10, 60);
_trail.Clear(); // prevent long streaks due to teleportation
}
/// <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)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
Matrix3x3 o;
Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
//Sample the local directions from the orientation matrix, implicitly transposed.
System.Numerics.Vector3 right;
var direction = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out right);
System.Numerics.Vector3 left;
direction = new System.Numerics.Vector3(-o.M11, -o.M21, -o.M31);
GetLocalExtremePointWithoutMargin(ref direction, out left);
System.Numerics.Vector3 up;
direction = new System.Numerics.Vector3(o.M12, o.M22, o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out up);
System.Numerics.Vector3 down;
direction = new System.Numerics.Vector3(-o.M12, -o.M22, -o.M32);
GetLocalExtremePointWithoutMargin(ref direction, out down);
System.Numerics.Vector3 backward;
direction = new System.Numerics.Vector3(o.M13, o.M23, o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out backward);
System.Numerics.Vector3 forward;
direction = new System.Numerics.Vector3(-o.M13, -o.M23, -o.M33);
GetLocalExtremePointWithoutMargin(ref direction, out forward);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
System.Numerics.Vector3 positive, negative;
TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);
//The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;
boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
}
public void xxxDrawMap()
{
float xs = XSize / Resolution;
float zs = ZSize / Resolution;
Vector3 alongRow = new Vector3(xs, 0, 0);
Vector3 nextRow = new Vector3(-XSize, 0, zs);
Vector3 g = new Vector3((XSize - xs) / -2, 0, (ZSize - zs) / -2);
g += Center;
for (int j = 0; j < Resolution; j++)
{
for (int i = 0; i < Resolution; i++)
{
if (GetMapBit(i, j))
{
// spikes
// Vector3 spikeTop (0, 5.0f, 0);
// drawLine (g, g+spikeTop, gWhite);
// squares
const float ROCK_HEIGHT = 0;
Vector3 v1 = new Vector3(+xs / 2, ROCK_HEIGHT, +zs / 2);
Vector3 v2 = new Vector3(+xs / 2, ROCK_HEIGHT, -zs / 2);
Vector3 v3 = new Vector3(-xs / 2, ROCK_HEIGHT, -zs / 2);
Vector3 v4 = new Vector3(-xs / 2, ROCK_HEIGHT, +zs / 2);
// Vector3 redRockColor (0.6f, 0.1f, 0.0f);
Color orangeRockColor = new Color((byte)(255.0f * 0.5f), (byte)(255.0f * 0.2f), (byte)(255.0f * 0.0f));
Drawing.DrawQuadrangle(g + v1, g + v2, g + v3, g + v4, orangeRockColor);
// pyramids
// Vector3 top (0, xs/2, 0);
// Vector3 redRockColor (0.6f, 0.1f, 0.0f);
// Vector3 orangeRockColor (0.5f, 0.2f, 0.0f);
// drawTriangle (g+v1, g+v2, g+top, redRockColor);
// drawTriangle (g+v2, g+v3, g+top, orangeRockColor);
// drawTriangle (g+v3, g+v4, g+top, redRockColor);
// drawTriangle (g+v4, g+v1, g+top, orangeRockColor);
}
g += alongRow;
}
g += nextRow;
}
}
// reset state
public override void Reset()
{
// reset vehicle state
base.Reset();
// speed along Forward direction.
Speed = _startSpeed;
// initial position along X axis
Position = new Vector3(_startX, 0, 0);
// for next instance: step starting location
_startX += 2;
// for next instance: step speed
_startSpeed += 0.15f;
// 15 seconds and 150 points along the trail
_trail = new Trail(15, 150);
}
// called when steerToFollowPath decides steering is required
public void AnnotatePathFollowing(Vector3 future, Vector3 onPath, Vector3 target, float outside)
{
Color yellow = Color.Yellow;
Color lightOrange = new Color((byte)(255.0f * 1.0f), (byte)(255.0f * 0.5f), 0);
Color darkOrange = new Color((byte)(255.0f * 0.6f), (byte)(255.0f * 0.3f), 0);
// draw line from our position to our predicted future position
annotation.Line(Position, future, yellow.ToVector3().FromXna());
// draw line from our position to our steering target on the path
annotation.Line(Position, target, Color.Orange.ToVector3().FromXna());
// draw a two-toned line between the future test point and its
// projection onto the path, the change from dark to light color
// indicates the boundary of the tube.
Vector3 boundaryOffset = Vector3.Normalize(onPath - future);
boundaryOffset *= outside;
Vector3 onPathBoundary = future + boundaryOffset;
annotation.Line(onPath, onPathBoundary, darkOrange.ToVector3().FromXna());
annotation.Line(onPathBoundary, future, lightOrange.ToVector3().FromXna());
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
public void AddNewSimplexPoint(ref System.Numerics.Vector3 point)
{
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
break;
}
}
///<summary>
/// Adds a new point to the simplex.
///</summary>
///<param name="point">Point to add.</param>
///<param name="hitLocation">Current ray hit location.</param>
///<param name="shiftedSimplex">Simplex shifted with the hit location.</param>
public void AddNewSimplexPoint(ref System.Numerics.Vector3 point, ref System.Numerics.Vector3 hitLocation, out RaySimplex shiftedSimplex)
{
shiftedSimplex = new RaySimplex();
switch (State)
{
case SimplexState.Empty:
State = SimplexState.Point;
A = point;
Vector3Ex.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
break;
case SimplexState.Point:
State = SimplexState.Segment;
B = point;
Vector3Ex.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3Ex.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
break;
case SimplexState.Segment:
State = SimplexState.Triangle;
C = point;
Vector3Ex.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3Ex.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3Ex.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
break;
case SimplexState.Triangle:
State = SimplexState.Tetrahedron;
D = point;
Vector3Ex.Subtract(ref hitLocation, ref A, out shiftedSimplex.A);
Vector3Ex.Subtract(ref hitLocation, ref B, out shiftedSimplex.B);
Vector3Ex.Subtract(ref hitLocation, ref C, out shiftedSimplex.C);
Vector3Ex.Subtract(ref hitLocation, ref D, out shiftedSimplex.D);
break;
}
shiftedSimplex.State = State;
}
/// <summary>
/// Constructs a compound collidable containing only the specified subset of children.
/// </summary>
/// <param name="shape">Shape to base the compound collidable on.</param>
/// <param name="childIndices">Indices of child shapes from the CompoundShape to include in the compound collidable.</param>
/// <returns>Compound collidable containing only the specified subset of children.</returns>
public static CompoundCollidable CreatePartialCompoundCollidable(CompoundShape shape, IList<int> childIndices)
{
if (childIndices.Count == 0)
throw new ArgumentException("Cannot create a compound from zero shapes.");
CompoundCollidable compound = new CompoundCollidable();
System.Numerics.Vector3 center = new System.Numerics.Vector3();
float totalWeight = 0;
for (int i = 0; i < childIndices.Count; i++)
{
//Create and add the child object itself.
var entry = shape.shapes[childIndices[i]];
compound.children.Add(new CompoundChild(shape, entry.Shape.GetCollidableInstance(), childIndices[i]));
//Grab its entry to compute the center of mass of this subset.
System.Numerics.Vector3 toAdd;
Vector3Ex.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd);
Vector3Ex.Add(ref center, ref toAdd, out center);
totalWeight += entry.Weight;
}
if (totalWeight <= 0)
{
throw new ArgumentException("Compound has zero total weight; invalid configuration.");
}
Vector3Ex.Divide(ref center, totalWeight, out center);
//Our subset of the compound is not necessarily aligned with the shape's origin.
//By default, an object will rotate around the center of the collision shape.
//We can't modify the shape data itself since it could be shared, which leaves
//modifying the local position of the collidable.
//We have the subset position in shape space, so pull the collidable back into alignment
//with the origin.
//This approach matches the rest of the CompoundHelper's treatment of subsets.
compound.LocalPosition = -center;
//Recompute the hierarchy for the compound.
compound.hierarchy.Tree.Reconstruct(compound.children);
compound.Shape = shape;
return compound;
}
protected override TriangleCollidable GetOpposingCollidable(int index)
{
//Construct a TriangleCollidable from the static mesh.
var toReturn = PhysicsResources.GetTriangleCollidable();
System.Numerics.Vector3 terrainUp = new System.Numerics.Vector3(mesh.worldTransform.LinearTransform.M21, mesh.worldTransform.LinearTransform.M22, mesh.worldTransform.LinearTransform.M23);
float dot;
System.Numerics.Vector3 AB, AC, normal;
var shape = toReturn.Shape;
mesh.Shape.GetTriangle(index, ref mesh.worldTransform, out shape.vA, out shape.vB, out shape.vC);
System.Numerics.Vector3 center;
Vector3Ex.Add(ref shape.vA, ref shape.vB, out center);
Vector3Ex.Add(ref center, ref shape.vC, out center);
Vector3Ex.Multiply(ref center, 1 / 3f, out center);
Vector3Ex.Subtract(ref shape.vA, ref center, out shape.vA);
Vector3Ex.Subtract(ref shape.vB, ref center, out shape.vB);
Vector3Ex.Subtract(ref shape.vC, ref center, out shape.vC);
//The bounding box doesn't update by itself.
toReturn.worldTransform.Position = center;
toReturn.worldTransform.Orientation = System.Numerics.Quaternion.Identity;
toReturn.UpdateBoundingBoxInternal(0);
Vector3Ex.Subtract(ref shape.vB, ref shape.vA, out AB);
Vector3Ex.Subtract(ref shape.vC, ref shape.vA, out AC);
Vector3Ex.Cross(ref AB, ref AC, out normal);
Vector3Ex.Dot(ref terrainUp, ref normal, out dot);
if (dot > 0)
{
shape.sidedness = TriangleSidedness.Clockwise;
}
else
{
shape.sidedness = TriangleSidedness.Counterclockwise;
}
shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin;
return toReturn;
}
/// <summary>
/// Updates the wheel's world transform for graphics.
/// Called automatically by the owning wheel at the end of each frame.
/// If the engine is updating asynchronously, you can call this inside of a space read buffer lock
/// and update the wheel transforms safely.
/// </summary>
public override void UpdateWorldTransform()
{
#if !WINDOWS
System.Numerics.Vector3 newPosition = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 newPosition;
#endif
System.Numerics.Vector3 worldAttachmentPoint;
System.Numerics.Vector3 localAttach;
Vector3Ex.Add(ref wheel.suspension.localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.localPosition, out localAttach);
worldTransform = Matrix3x3.ToMatrix4X4(wheel.vehicle.Body.BufferedStates.InterpolatedStates.OrientationMatrix);
Matrix4x4Ex.TransformNormal(ref localAttach, ref worldTransform, out worldAttachmentPoint);
worldAttachmentPoint += wheel.vehicle.Body.BufferedStates.InterpolatedStates.Position;
System.Numerics.Vector3 worldDirection;
Matrix4x4Ex.Transform(ref wheel.suspension.localDirection, ref worldTransform, out worldDirection);
float length = wheel.suspension.currentLength - graphicalRadius;
newPosition.X = worldAttachmentPoint.X + worldDirection.X * length;
newPosition.Y = worldAttachmentPoint.Y + worldDirection.Y * length;
newPosition.Z = worldAttachmentPoint.Z + worldDirection.Z * length;
System.Numerics.Matrix4x4 spinTransform;
System.Numerics.Vector3 localSpinAxis;
Vector3Ex.Cross(ref wheel.localForwardDirection, ref wheel.suspension.localDirection, out localSpinAxis);
Matrix4x4Ex.CreateFromAxisAngle(ref localSpinAxis, spinAngle, out spinTransform);
System.Numerics.Matrix4x4 localTurnTransform;
Matrix4x4Ex.Multiply(ref localGraphicTransform, ref spinTransform, out localTurnTransform);
Matrix4x4Ex.Multiply(ref localTurnTransform, ref steeringTransform, out localTurnTransform);
//System.Numerics.Matrix4x4.Multiply(ref localTurnTransform, ref spinTransform, out localTurnTransform);
Matrix4x4Ex.Multiply(ref localTurnTransform, ref worldTransform, out worldTransform);
worldTransform.Translation += newPosition;
}
/// <summary>
/// Builds a new twist limit. Prevents two bones from rotating beyond a certain angle away from each other as measured by attaching an axis to each connected bone.
/// </summary>
/// <param name="connectionA">First connection of the limit.</param>
/// <param name="connectionB">Second connection of the limit.</param>
/// <param name="axisA">Axis attached to connectionA in world space.</param>
/// <param name="axisB">Axis attached to connectionB in world space.</param>
/// <param name="maximumAngle">Maximum angle allowed between connectionA's axis and connectionB's axis.</param>
public IKTwistLimit(Bone connectionA, Bone connectionB, System.Numerics.Vector3 axisA, System.Numerics.Vector3 axisB, float maximumAngle)
: base(connectionA, connectionB)
{
AxisA = axisA;
AxisB = axisB;
MaximumAngle = maximumAngle;
ComputeMeasurementAxes();
}
/// <summary>
/// Constructs a nondynamic capsule.
/// </summary>
/// <param name="position">Position of the capsule.</param>
/// <param name="length">Length of the capsule.</param>
/// <param name="radius">Radius of the capsule.</param>
public Capsule(System.Numerics.Vector3 position, float length, float radius)
: this(length, radius)
{
Position = position;
}
///<summary>
/// Gets the extreme point of the shape in local space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public override void GetLocalExtremePointWithoutMargin(ref System.Numerics.Vector3 direction, out System.Numerics.Vector3 extremePoint)
{
shapes.WrappedList.Elements[0].CollisionShape.GetExtremePoint(direction, ref shapes.WrappedList.Elements[0].Transform, out extremePoint);
float maxDot;
Vector3Ex.Dot(ref extremePoint, ref direction, out maxDot);
for (int i = 1; i < shapes.WrappedList.Count; i++)
{
float dot;
System.Numerics.Vector3 temp;
shapes.WrappedList.Elements[i].CollisionShape.GetExtremePoint(direction, ref shapes.WrappedList.Elements[i].Transform, out temp);
Vector3Ex.Dot(ref direction, ref temp, out dot);
if (dot > maxDot)
{
extremePoint = temp;
maxDot = dot;
}
}
}
/// <summary>
/// Constructs a new bounding sphere.
/// </summary>
/// <param name="center">Location of the center of the sphere.</param>
/// <param name="radius">Radius of the sphere.</param>
public BoundingSphere(System.Numerics.Vector3 center, float radius)
{
this.Center = center;
this.Radius = radius;
}
/// <summary>
/// Gets the linear jacobian entry for the first connected entity.
/// </summary>
/// <param name="jacobian">Linear jacobian entry for the first connected entity.</param>
public void GetLinearJacobianA(out System.Numerics.Vector3 jacobian)
{
jacobian = Toolbox.ZeroVector;
}
public static UnityEngine.Vector3 SystemVector3ToUnity(System.Numerics.Vector3 vector)
{
return(new UnityEngine.Vector3(vector.X, vector.Y, -vector.Z));
}
/// <summary>
/// Gets the world space position of a vertex in the terrain at the given indices.
/// </summary>
///<param name="columnIndex">Index in the first dimension.</param>
///<param name="rowIndex">Index in the second dimension.</param>
/// <param name="transform">Transform to apply to the vertex.</param>
/// <param name="position">Transformed position of the vertex at the given indices.</param>
public void GetPosition(int columnIndex, int rowIndex, ref AffineTransform transform, out System.Numerics.Vector3 position)
{
if (columnIndex <= 0)
{
columnIndex = 0;
}
else if (columnIndex >= heights.GetLength(0))
{
columnIndex = heights.GetLength(0) - 1;
}
if (rowIndex <= 0)
{
rowIndex = 0;
}
else if (rowIndex >= heights.GetLength(1))
{
rowIndex = heights.GetLength(1) - 1;
}
#if !WINDOWS
position = new System.Numerics.Vector3();
#endif
position.X = columnIndex;
position.Y = heights[columnIndex, rowIndex];
position.Z = rowIndex;
AffineTransform.Transform(ref position, ref transform, out position);
}
///<summary>
/// Tests a ray against the terrain shape.
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="transform">Transform to apply to the terrain shape during the test.</param>
///<param name="sidedness">Sidedness of the triangles to use when raycasting.</param>
///<param name="hit">Hit data of the ray cast, if any.</param>
///<returns>Whether or not the ray hit the transformed terrain shape.</returns>
public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit)
{
hit = new RayHit();
//Put the ray into local space.
Ray localRay;
AffineTransform inverse;
AffineTransform.Invert(ref transform, out inverse);
Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);
//Use rasterizey traversal.
//The origin is at 0,0,0 and the map goes +X, +Y, +Z.
//if it's before the origin and facing away, or outside the max and facing out, early out.
float maxX = heights.GetLength(0) - 1;
float maxZ = heights.GetLength(1) - 1;
System.Numerics.Vector3 progressingOrigin = localRay.Position;
float distance = 0;
//Check the outside cases first.
if (progressingOrigin.X < 0)
{
if (localRay.Direction.X > 0)
{
//Off the left side.
float timeToMinX = -progressingOrigin.X / localRay.Direction.X;
distance += timeToMinX;
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
else if (progressingOrigin.X > maxX)
{
if (localRay.Direction.X < 0)
{
//Off the left side.
float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X;
distance += timeToMinX;
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToMinX, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false); //Outside and pointing away from the terrain.
}
}
if (progressingOrigin.Z < 0)
{
if (localRay.Direction.Z > 0)
{
float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z;
distance += timeToMinZ;
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
else if (progressingOrigin.Z > maxZ)
{
if (localRay.Direction.Z < 0)
{
float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z;
distance += timeToMinZ;
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToMinZ, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
return(false);
}
}
if (distance > maximumLength)
{
return(false);
}
//By now, we should be entering the main body of the terrain.
int xCell = (int)progressingOrigin.X;
int zCell = (int)progressingOrigin.Z;
//If it's hitting the border and going in, then correct the index
//so that it will initially target a valid quad.
//Without this, a quad beyond the border would be tried and failed.
if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0)
{
xCell = heights.GetLength(0) - 2;
}
if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0)
{
zCell = heights.GetLength(1) - 2;
}
while (true)
{
//Check for a miss.
if (xCell < 0 ||
zCell < 0 ||
xCell >= heights.GetLength(0) - 1 ||
zCell >= heights.GetLength(1) - 1)
{
return(false);
}
//Test the triangles of this cell.
System.Numerics.Vector3 v1, v2, v3, v4;
// v3 v4
// v1 v2
GetLocalPosition(xCell, zCell, out v1);
GetLocalPosition(xCell + 1, zCell, out v2);
GetLocalPosition(xCell, zCell + 1, out v3);
GetLocalPosition(xCell + 1, zCell + 1, out v4);
RayHit hit1, hit2;
bool didHit1;
bool didHit2;
//Don't bother doing ray intersection tests if the ray can't intersect it.
float highest = v1.Y;
float lowest = v1.Y;
if (v2.Y > highest)
{
highest = v2.Y;
}
else if (v2.Y < lowest)
{
lowest = v2.Y;
}
if (v3.Y > highest)
{
highest = v3.Y;
}
else if (v3.Y < lowest)
{
lowest = v3.Y;
}
if (v4.Y > highest)
{
highest = v4.Y;
}
else if (v4.Y < lowest)
{
lowest = v4.Y;
}
if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 ||
progressingOrigin.Y < lowest && localRay.Direction.Y < 0))
{
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
//Always perform the raycast as if Y+ in local space is the way the triangles are facing.
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2);
}
else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft)
{
didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1);
didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2);
}
if (didHit1 && didHit2)
{
if (hit1.T < hit2.T)
{
Vector3Ex.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
Vector3Ex.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
else if (didHit1)
{
Vector3Ex.Multiply(ref ray.Direction, hit1.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit1.T;
return(true);
}
else if (didHit2)
{
Vector3Ex.Multiply(ref ray.Direction, hit2.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
hit.T = hit2.T;
return(true);
}
}
//Move to the next cell.
float timeToX;
if (localRay.Direction.X < 0)
{
timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X;
}
else if (ray.Direction.X > 0)
{
timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X;
}
else
{
timeToX = float.MaxValue;
}
float timeToZ;
if (localRay.Direction.Z < 0)
{
timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z;
}
else if (localRay.Direction.Z > 0)
{
timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z;
}
else
{
timeToZ = float.MaxValue;
}
//Move to the next cell.
if (timeToX < timeToZ)
{
if (localRay.Direction.X < 0)
{
xCell--;
}
else
{
xCell++;
}
distance += timeToX;
if (distance > maximumLength)
{
return(false);
}
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToX, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
else
{
if (localRay.Direction.Z < 0)
{
zCell--;
}
else
{
zCell++;
}
distance += timeToZ;
if (distance > maximumLength)
{
return(false);
}
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref localRay.Direction, timeToZ, out increment);
Vector3Ex.Add(ref increment, ref progressingOrigin, out progressingOrigin);
}
}
}
private void Update(EvaluationContext context)
{
if (!(SourcePoints.GetValue(context) is StructuredList <Point> sourcePoints))
{
return;
}
if (sourcePoints.NumElements == 0)
{
sourcePoints.SetLength(0);
ResultList.Value = sourcePoints;
return;
}
var spread = Spread.GetValue(context);
var spreadMode = (SpreadModes)SpreadMode.GetValue(context);
var indexWithinSegment = 0;
var lineSegmentLength = 0f;
var totalLength = 0f;
var maxLength = float.NegativeInfinity;
var randomizeStart = RandomizeStart.GetValue(context);
var randomizeDuration = RandomizeDuration.GetValue(context);
// Measure...
segments.Clear();
for (var pointIndex = 0; pointIndex < sourcePoints.NumElements; pointIndex++)
{
if (float.IsNaN(sourcePoints.TypedElements[pointIndex].W))
{
var hasAtLeastTwoPoints = indexWithinSegment > 1;
if (hasAtLeastTwoPoints)
{
if (lineSegmentLength > maxLength)
{
maxLength = lineSegmentLength;
}
totalLength += lineSegmentLength;
segments.Add(new Segment
{
PointIndex = pointIndex - indexWithinSegment,
PointCount = indexWithinSegment,
AccumulatedLength = totalLength,
SegmentLength = lineSegmentLength
});
}
lineSegmentLength = 0;
indexWithinSegment = 0;
}
else
{
if (indexWithinSegment > 0)
{
lineSegmentLength += Vector3.Distance(sourcePoints.TypedElements[pointIndex - 1].Position,
sourcePoints.TypedElements[pointIndex].Position);
}
indexWithinSegment++;
}
}
if (totalLength < 0.0001f || segments.Count < 2)
{
Log.Warning("Stroke animation requires at least two segments with of some length");
return;
}
// Write offsets...
float dist = maxLength / (segments.Count - 1);
_random = new Random(42);
for (var segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
{
var segmentOffset = ComputeOverlappingProgress(0, segmentIndex, segments.Count, spread);
var lengthProgressWithingSegment = 0f;
var segment = segments[segmentIndex];
// see https://www.figma.com/file/V5k13NMMIsnAnbWH651clI/Untitled?node-id=205%3A96
var stackedRange = TimeRange.FromStartAndDuration(segment.AccumulatedLength - segment.SegmentLength, segment.SegmentLength) * (1 / totalLength);
var anchor = segmentIndex * segment.SegmentLength / (segments.Count - 1);
var pGrid = segmentIndex * dist;
var packedRange = TimeRange.FromStartAndDuration(pGrid - anchor, segment.SegmentLength) * (1 / maxLength);
var range = TimeRange.Lerp(packedRange, stackedRange, spread);
if (Math.Abs(randomizeStart) > 0.0001f)
{
var randomStart = (float)_random.NextDouble() * (1 - range.Duration);
range.Start = MathUtils.Lerp(range.Start, randomStart, randomizeStart);
}
if (Math.Abs(randomizeDuration) > 0.0001f)
{
var randomDuration = (float)_random.NextDouble() * (1 - range.Start);
range.Duration = MathUtils.Lerp(range.Duration, randomDuration, randomizeDuration);
}
for (var pointIndexInSegment = 0; pointIndexInSegment < segment.PointCount; pointIndexInSegment++)
{
var pi = segment.PointIndex + pointIndexInSegment;
if (pointIndexInSegment > 0)
{
lengthProgressWithingSegment += Vector3.Distance(sourcePoints.TypedElements[pi - 1].Position,
sourcePoints.TypedElements[pi].Position);
}
var normalizedSegmentPosition = pointIndexInSegment / (segment.PointCount - 1);
float w = 0;
switch (spreadMode)
{
case SpreadModes.IgnoreStrokeLengths:
var f = lengthProgressWithingSegment / segment.SegmentLength.Clamp(0.001f, 999999f);
w = (f - segmentOffset) / (segments.Count + 1);
break;
case SpreadModes.UseStrokeLength:
w = MathUtils.Lerp(range.Start, range.End, normalizedSegmentPosition);
break;
case SpreadModes.Weird:
w = segmentOffset * 0.2f + pointIndexInSegment / segment.PointCount / 2;
break;
}
sourcePoints.TypedElements[pi].W = w;
}
}
StrokeCount.Value = segments.Count;
ResultList.Value = sourcePoints;
StrokeCount.DirtyFlag.Clear();
ResultList.DirtyFlag.Clear();
}
/// <summary>
/// Initializes a new instance of the <see cref="TrajectoryPose"/> for mocking purposes.
/// </summary>
/// <param name="rotation"> The pose's rotation. </param>
/// <param name="translation"> The pose's translation. </param>
/// <returns> A new instance of the <see cref="TrajectoryPose"/> for mocking purposes. </returns>
public static TrajectoryPose TrajectoryPose(System.Numerics.Quaternion rotation, System.Numerics.Vector3 translation)
{
return(new TrajectoryPose(rotation, translation));
}
/// <summary>
/// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
/// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public PrismaticJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 lineAnchor, System.Numerics.Vector3 lineDirection, System.Numerics.Vector3 pointAnchor)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
AngularJoint = new NoRotationJoint(connectionA, connectionB);
Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
Limit.IsActive = false;
Motor.IsActive = false;
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
protected override void OnTransformScaleManuallyChanged(System.Numerics.Vector3 obj)
{
SetPhysicsBoxDimensions();
}
public static UnityEngine.Vector3 SystemVector3ToUnity(System.Numerics.Vector3 vector)
{
return(vector.ToUnityVector3());
}
private static Vector3 WindowsVectorToUnityVector(WindowsVector3 v)
{
return(new Vector3(v.X, v.Y, -v.Z));
}
public static ImportedAnimation ImportFromAssimpScene(Scene scene,
AnimationImportSettings settings)
{
ImportedAnimation result = new ImportedAnimation();
var sceneMatrix = System.Numerics.Matrix4x4.CreateScale(System.Numerics.Vector3.One * settings.SceneScale);
if (!settings.FlipQuaternionHandedness)
{
sceneMatrix *= System.Numerics.Matrix4x4.CreateScale(-1, 1, 1);
}
if (settings.ExistingHavokAnimationTemplate == null)
{
throw new NotImplementedException("Reading skeleton/binding from assimp scene not supported yet. Please import using existing havok animation as template.");
}
else
{
result.hkaSkeleton = settings.ExistingHavokAnimationTemplate.hkaSkeleton;
result.HkxBoneIndexToTransformTrackMap = settings.ExistingHavokAnimationTemplate.HkxBoneIndexToTransformTrackMap;
result.TransformTrackIndexToHkxBoneMap = settings.ExistingHavokAnimationTemplate.TransformTrackIndexToHkxBoneMap;
}
if (settings.ConvertFromZUp)
{
sceneMatrix *= System.Numerics.Matrix4x4.CreateRotationZ((float)(Math.PI));
sceneMatrix *= System.Numerics.Matrix4x4.CreateRotationX((float)(-Math.PI / 2.0));
}
foreach (var anim in scene.Animations)
{
if (anim.HasNodeAnimations)
{
// Setup framerate.
double tickScaler = (settings.ResampleToFramerate / anim.TicksPerSecond);
result.Duration = anim.DurationInTicks != 0 ? // Don't divide by 0
(float)(anim.DurationInTicks / anim.TicksPerSecond) : 0;
result.FrameDuration = (float)(1 / settings.ResampleToFramerate);
int frameCount = (int)Math.Round((anim.DurationInTicks / anim.TicksPerSecond) * settings.ResampleToFramerate);
double resampleTickMult = settings.ResampleToFramerate / anim.TicksPerSecond;
Dictionary <string, int> transformTrackIndexRemapForExistingBoneNameList
= new Dictionary <string, int>();
List <string> transformTrackNames = new List <string>();
// Populate transform track names.
foreach (var nodeChannel in anim.NodeAnimationChannels)
{
if (nodeChannel.NodeName == settings.RootMotionNodeName)
{
continue;
}
// If we have a predefined transform track list
// e.g. for an existing character, then just give the
// info needed to remap
if (settings.ExistingBoneNameList != null)
{
transformTrackIndexRemapForExistingBoneNameList.Add(nodeChannel.NodeName,
settings.ExistingBoneNameList.IndexOf(nodeChannel.NodeName));
}
else
{
transformTrackNames.Add(nodeChannel.NodeName);
}
}
result.TransformTrackNames = settings.ExistingBoneNameList != null?
transformTrackNames.OrderBy(x => settings.ExistingBoneNameList.IndexOf(x)).ToList()
: transformTrackNames;
if (settings.ExistingBoneNameList != null)
{
transformTrackNames.Clear();
foreach (var name in settings.ExistingBoneNameList)
{
transformTrackNames.Add(name);
}
}
result.TransformTrackNames = transformTrackNames;
result.Frames = new List <ImportedAnimation.Frame>();
for (int i = 0; i <= frameCount; i++)
{
var f = new ImportedAnimation.Frame();
for (int j = 0; j < transformTrackNames.Count; j++)
{
f.BoneTransforms.Add(NewBlendableTransform.Identity);
}
result.Frames.Add(f);
}
for (int i = 0; i < anim.NodeAnimationChannelCount; i++)
{
var nodeChannel = anim.NodeAnimationChannels[i];
int lastKeyIndex = -1;
if (nodeChannel.NodeName == settings.RootMotionNodeName)
{
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.PositionKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
result.Frames[frame].RootMotionTranslation =
System.Numerics.Vector3.Transform(keyPos.Value.ToNumerics(), sceneMatrix);
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].RootMotionTranslation;
var blendTo = result.Frames[frame].RootMotionTranslation;
result.Frames[f].RootMotionTranslation = System.Numerics.Vector3.Lerp(blendFrom, blendTo, lerpS);
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
result.Frames[f].RootMotionTranslation = result.Frames[lastKeyIndex].RootMotionTranslation;
}
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.RotationKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var quatOnFrame = keyPos.Value.ToNumerics();
quatOnFrame.Y *= -1;
quatOnFrame.Z *= -1;
System.Numerics.Vector3 directionVectorOnFrame =
System.Numerics.Vector3.Transform(System.Numerics.Vector3.UnitX, quatOnFrame);
float angleOnFrame = (float)Math.Atan2(directionVectorOnFrame.Z, directionVectorOnFrame.X);
if (settings.FlipQuaternionHandedness)
{
angleOnFrame = -angleOnFrame;
}
result.Frames[frame].RootMotionRotation = angleOnFrame;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].RootMotionTranslation;
var blendTo = result.Frames[frame].RootMotionTranslation;
result.Frames[f].RootMotionRotation =
SapMath.Lerp(result.Frames[lastKeyIndex].RootMotionRotation, angleOnFrame, lerpS);
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
result.Frames[f].RootMotionRotation = result.Frames[lastKeyIndex].RootMotionRotation;
}
}
else
{
int transformIndex = settings.ExistingBoneNameList != null ?
transformTrackIndexRemapForExistingBoneNameList[nodeChannel.NodeName] : i;
if (transformIndex >= 0 && transformIndex < transformTrackNames.Count)
{
// TRANSLATION
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.PositionKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Translation = System.Numerics.Vector3.Transform(keyPos.Value.ToNumerics(), sceneMatrix);
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Translation;
var blendTo = curFrameTransform.Translation;
var blended = System.Numerics.Vector3.Lerp(blendFrom, blendTo, lerpS);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Translation = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Translation = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Translation;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
// SCALE
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.ScalingKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Scale = keyPos.Value.ToNumerics();
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Scale;
var blendTo = curFrameTransform.Scale;
var blended = System.Numerics.Vector3.Lerp(blendFrom, blendTo, lerpS);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Scale = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Scale = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Scale;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
// ROTATION
lastKeyIndex = -1;
foreach (var keyPos in nodeChannel.RotationKeys)
{
int frame = (int)Math.Floor(keyPos.Time * resampleTickMult);
var curFrameTransform = result.Frames[frame].BoneTransforms[transformIndex];
curFrameTransform.Rotation = keyPos.Value.ToNumerics();
curFrameTransform.Rotation.Y *= -1;
curFrameTransform.Rotation.Z *= -1;
if (settings.FlipQuaternionHandedness)
{
curFrameTransform.Rotation = SapMath.MirrorQuat(curFrameTransform.Rotation);
}
result.Frames[frame].BoneTransforms[transformIndex] = curFrameTransform;
// Fill in from the last keyframe to this one
for (int f = lastKeyIndex + 1; f <= Math.Min(frame - 1, result.Frames.Count - 1); f++)
{
float lerpS = 1f * (f - lastKeyIndex) / (frame - lastKeyIndex);
var blendFrom = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Rotation;
var blendTo = curFrameTransform.Rotation;
var blended = System.Numerics.Quaternion.Slerp(blendFrom, blendTo, lerpS);
var copyOfStruct = result.Frames[f].BoneTransforms[transformIndex];
copyOfStruct.Rotation = blended;
result.Frames[f].BoneTransforms[transformIndex] = copyOfStruct;
}
lastKeyIndex = frame;
}
// Fill in from last key to end of animation.
for (int f = lastKeyIndex + 1; f <= result.Frames.Count - 1; f++)
{
var x = result.Frames[f].BoneTransforms[transformIndex];
x.Rotation = result.Frames[lastKeyIndex].BoneTransforms[transformIndex].Rotation;
result.Frames[f].BoneTransforms[transformIndex] = x;
}
}
}
}
result.FrameCount = frameCount;
break;
}
}
result.RootMotion = new RootMotionData(
new System.Numerics.Vector4(0, 1, 0, 0),
new System.Numerics.Vector4(0, 0, 1, 0),
result.Duration, result.Frames.Select(f => f.RootMotion).ToArray());
// Copy first frame for loop?
//for (int i = 0; i < result.TransformTrackNames.Count; i++)
//{
// result.Frames[result.Frames.Count - 1].BoneTransforms[i] = result.Frames[0].BoneTransforms[i];
//}
result.Name = settings.ExistingHavokAnimationTemplate?.Name ?? "SAP Custom Animation";
return(result);
}
///<summary>
/// Gets the second triangle of the quad at the given indices in world space.
///</summary>
///<param name="columnIndex">Index of the triangle's quad in the first dimension.</param>
///<param name="rowIndex">Index of the triangle's quad in the second dimension.</param>
///<param name="transform">Transform to apply to the triangle vertices.</param>
///<param name="a">First vertex of the triangle.</param>
///<param name="b">Second vertex of the triangle.</param>
///<param name="c">Third vertex of the triangle.</param>
public void GetSecondTriangle(int columnIndex, int rowIndex, ref AffineTransform transform, out System.Numerics.Vector3 a, out System.Numerics.Vector3 b, out System.Numerics.Vector3 c)
{
if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
{
GetPosition(columnIndex, rowIndex + 1, ref transform, out a);
GetPosition(columnIndex + 1, rowIndex + 1, ref transform, out b);
GetPosition(columnIndex + 1, rowIndex, ref transform, out c);
}
else
{
GetPosition(columnIndex, rowIndex, ref transform, out a);
GetPosition(columnIndex, rowIndex + 1, ref transform, out b);
GetPosition(columnIndex + 1, rowIndex + 1, ref transform, out c);
}
}
static public void Write(this System.IO.BinaryWriter self, System.Numerics.Vector3 vec)
{
self.Write(vec.X);
self.Write(vec.Y);
self.Write(vec.Z);
}
///<summary>
/// Gets a world space triangle in the terrain at the given triangle index.
///</summary>
///<param name="index">Index of the triangle. Encoded as 2 * (quadRowIndex * terrainWidthInQuads + quadColumnIndex) + isFirstTriangleOfQuad ? 0 : 1, where isFirstTriangleOfQuad refers to which of the two triangles in a quad is being requested. Matches the output of the TerrainShape.GetOverlaps function.</param>
///<param name="transform">Transform to apply to the triangle vertices.</param>
///<param name="a">First vertex of the triangle.</param>
///<param name="b">Second vertex of the triangle.</param>
///<param name="c">Third vertex of the triangle.</param>
public void GetTriangle(int index, ref AffineTransform transform, out System.Numerics.Vector3 a, out System.Numerics.Vector3 b, out System.Numerics.Vector3 c)
{
//Find the quad.
int quadIndex = index / 2;
//TODO: This division could be avoided if you're willing to get tricky or impose some size requirements.
int rowIndex = quadIndex / heights.GetLength(0);
int columnIndex = quadIndex - rowIndex * heights.GetLength(0);
if ((index & 1) == 0) //Check if this is the first or second triangle.
{
GetFirstTriangle(columnIndex, rowIndex, ref transform, out a, out b, out c);
}
else
{
GetSecondTriangle(columnIndex, rowIndex, ref transform, out a, out b, out c);
}
}
public static void Write(byte[] bytes, int i, System.Numerics.Vector3 value)
{
Write(bytes, i, value.X);
Write(bytes, i + 4, value.Y);
Write(bytes, i + 8, value.Z);
}
///<summary>
/// Constructs the bounding box of the terrain given a transform.
///</summary>
///<param name="transform">Transform to apply to the terrain during the bounding box calculation.</param>
///<param name="boundingBox">Bounding box of the terrain shape when transformed.</param>
public void GetBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
float minX = float.MaxValue, maxX = -float.MaxValue,
minY = float.MaxValue, maxY = -float.MaxValue,
minZ = float.MaxValue, maxZ = -float.MaxValue;
System.Numerics.Vector3 minXvertex = new System.Numerics.Vector3(),
maxXvertex = new System.Numerics.Vector3(),
minYvertex = new System.Numerics.Vector3(),
maxYvertex = new System.Numerics.Vector3(),
minZvertex = new System.Numerics.Vector3(),
maxZvertex = new System.Numerics.Vector3();
//Find the extreme locations.
for (int i = 0; i < heights.GetLength(0); i++)
{
for (int j = 0; j < heights.GetLength(1); j++)
{
var vertex = new System.Numerics.Vector3(i, heights[i, j], j);
Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex);
if (vertex.X < minX)
{
minX = vertex.X;
minXvertex = vertex;
}
else if (vertex.X > maxX)
{
maxX = vertex.X;
maxXvertex = vertex;
}
if (vertex.Y < minY)
{
minY = vertex.Y;
minYvertex = vertex;
}
else if (vertex.Y > maxY)
{
maxY = vertex.Y;
maxYvertex = vertex;
}
if (vertex.Z < minZ)
{
minZ = vertex.Z;
minZvertex = vertex;
}
else if (vertex.Z > maxZ)
{
maxZ = vertex.Z;
maxZvertex = vertex;
}
}
}
//Shift the bounding box.
boundingBox.Min.X = minXvertex.X + transform.Translation.X;
boundingBox.Min.Y = minYvertex.Y + transform.Translation.Y;
boundingBox.Min.Z = minZvertex.Z + transform.Translation.Z;
boundingBox.Max.X = maxXvertex.X + transform.Translation.X;
boundingBox.Max.Y = maxYvertex.Y + transform.Translation.Y;
boundingBox.Max.Z = maxZvertex.Z + transform.Translation.Z;
}
/// <summary>
/// Constructs a convex shape entry.
/// </summary>
/// <param name="position">Local position of the entry.</param>
/// <param name="shape">Shape of the entry.</param>
public ConvexShapeEntry(System.Numerics.Vector3 position, ConvexShape shape)
{
Transform = new RigidTransform(position);
CollisionShape = shape;
}
public bool ImportMorphTargets(FileInfo inGltfFile, Stream intargetStream, List <Archive> archives, ValidationMode vmode = ValidationMode.Strict, Stream outStream = null)
{
var cr2w = _wolvenkitFileService.ReadRed4File(intargetStream);
if (cr2w == null || cr2w.RootChunk is not MorphTargetMesh blob || blob.Blob.Chunk is not rendRenderMorphTargetMeshBlob renderblob || renderblob.BaseBlob.Chunk is not rendRenderMeshBlob rendblob)
{
return(false);
}
RawArmature newRig = null;
{
var hash = FNV1A64HashAlgorithm.HashString(blob.BaseMesh.DepotPath);
var meshStream = new MemoryStream();
foreach (var ar in archives)
{
if (ar.Files.ContainsKey(hash))
{
ExtractSingleToStream(ar, hash, meshStream);
break;
}
}
var meshCr2w = _wolvenkitFileService.ReadRed4File(meshStream);
if (meshCr2w != null && meshCr2w.RootChunk is CMesh mesh && mesh.RenderResourceBlob.Chunk is rendRenderMeshBlob rendBlob)
{
newRig = MeshTools.GetOrphanRig(rendBlob, meshCr2w);
}
}
var model = ModelRoot.Load(inGltfFile.FullName, new ReadSettings(vmode));
VerifyGLTF(model);
var submeshCount = model.LogicalMeshes.Count;
if (submeshCount == 0)
{
throw new Exception("No submeshes found in model file.");
}
using var diffsBuffer = new MemoryStream();
using var mappingsBuffer = new MemoryStream();
// Deserialize mappings buffer
/*if (renderblob.MappingBuffer.IsSerialized)
* {
* intargetStream.Seek(cr2w.Buffers[mappingsBufferId].Offset, SeekOrigin.Begin);
* intargetStream.DecompressAndCopySegment(mappingsBuffer, cr2w.Buffers[mappingsBufferId].DiskSize, cr2w.Buffers[mappingsBufferId].MemSize);
* }*/
// Zero out some values that will be set later
renderblob.Header.NumDiffs = 0;
for (var i = 0; i < renderblob.Header.TargetStartsInVertexDiffs.Count; i++)
{
renderblob.Header.TargetStartsInVertexDiffs[i] = 0;
}
for (var i = 0; i < renderblob.Header.TargetStartsInVertexDiffsMapping.Count; i++)
{
renderblob.Header.TargetStartsInVertexDiffsMapping[i] = 0;
}
SetTargets(cr2w, model, renderblob, diffsBuffer, mappingsBuffer);
renderblob.DiffsBuffer.Buffer.SetBytes(diffsBuffer.ToArray());
renderblob.MappingBuffer.Buffer.SetBytes(mappingsBuffer.ToArray());
VerifyGLTF(model);
var Meshes = new List <RawMeshContainer>();
for (var i = 0; i < model.LogicalMeshes.Count; i++)
{
Meshes.Add(GltfMeshToRawContainer(model.LogicalMeshes[i]));
}
var max = new Vec3(Meshes[0].positions[0].X, Meshes[0].positions[0].Y, Meshes[0].positions[0].Z);
var min = new Vec3(Meshes[0].positions[0].X, Meshes[0].positions[0].Y, Meshes[0].positions[0].Z);
for (var e = 0; e < Meshes.Count; e++)
{
for (var i = 0; i < Meshes[e].positions.Length; i++)
{
if (Meshes[e].positions[i].X >= max.X)
{
max.X = Meshes[e].positions[i].X;
}
if (Meshes[e].positions[i].Y >= max.Y)
{
max.Y = Meshes[e].positions[i].Y;
}
if (Meshes[e].positions[i].Z >= max.Z)
{
max.Z = Meshes[e].positions[i].Z;
}
if (Meshes[e].positions[i].X <= min.X)
{
min.X = Meshes[e].positions[i].X;
}
if (Meshes[e].positions[i].Y <= min.Y)
{
min.Y = Meshes[e].positions[i].Y;
}
if (Meshes[e].positions[i].Z <= min.Z)
{
min.Z = Meshes[e].positions[i].Z;
}
}
}
// updating bounding box
blob.BoundingBox.Min.X = min.X;
blob.BoundingBox.Min.Y = min.Y;
blob.BoundingBox.Min.Z = min.Z;
blob.BoundingBox.Max.X = max.X;
blob.BoundingBox.Max.Y = max.Y;
blob.BoundingBox.Max.Z = max.Z;
var QuantScale = new Vec4((max.X - min.X) / 2, (max.Y - min.Y) / 2, (max.Z - min.Z) / 2, 0);
var QuantTrans = new Vec4((max.X + min.X) / 2, (max.Y + min.Y) / 2, (max.Z + min.Z) / 2, 1);
RawArmature oldRig = null;
if (model.LogicalSkins.Count != 0)
{
oldRig = new RawArmature
{
Names = new string[model.LogicalSkins[0].JointsCount]
};
for (var i = 0; i < model.LogicalSkins[0].JointsCount; i++)
{
oldRig.Names[i] = model.LogicalSkins[0].GetJoint(i).Joint.Name;
}
}
MeshTools.UpdateMeshJoints(ref Meshes, newRig, oldRig);
var expMeshes = new List <Re4MeshContainer>();
for (var i = 0; i < Meshes.Count; i++)
{
expMeshes.Add(RawMeshToRE4Mesh(Meshes[i], QuantScale, QuantTrans));
}
var meshBuffer = new MemoryStream();
var meshesInfo = BufferWriter(expMeshes, ref meshBuffer);
meshesInfo.quantScale = QuantScale;
meshesInfo.quantTrans = QuantTrans;
var ms = GetEditedCr2wFile(cr2w, meshesInfo, meshBuffer);
ms.Seek(0, SeekOrigin.Begin);
if (outStream != null)
{
ms.CopyTo(outStream);
}
else
{
intargetStream.SetLength(0);
ms.CopyTo(intargetStream);
}
return(true);
}
///<summary>
/// Constructs a wrapped shape.
/// A constructor is also available which takes a list of objects rather than just a pair.
/// The shape will be recentered.
///</summary>
///<param name="firstShape">First shape in the wrapped shape.</param>
///<param name="secondShape">Second shape in the wrapped shape.</param>
///<param name="center">Center of the shape before recentering..</param>
public WrappedShape(ConvexShapeEntry firstShape, ConvexShapeEntry secondShape, out System.Numerics.Vector3 center)
{
shapes.Add(firstShape);
shapes.Add(secondShape);
UpdateConvexShapeInfo(out center);
shapes.Changed += ShapesChanged;
}
private bool DoDeepContact(out ContactData contact)
{
#region Informed search
if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal.
{
//It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space!
//System.Numerics.Vector3 velocityA;
//Vector3Ex.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA);
//Vector3Ex.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA);
//System.Numerics.Vector3 velocityB;
//Vector3Ex.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB);
//Vector3Ex.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB);
//Vector3Ex.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB);
////The velocity is negated because the direction so point backwards along the velocity.
//Vector3Ex.Subtract(ref velocityA, ref velocityB, out localDirection);
//The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine.
if (collidableA.entity != null && collidableB.entity != null)
{
Vector3Ex.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection);
}
else
{
localDirection = localSeparatingAxis;
}
if (localDirection.LengthSquared() < Toolbox.Epsilon)
{
localDirection = Vector3Ex.Up;
}
}
if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact))
{
if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
{
state = CollisionState.ShallowContact;
}
return(true);
}
//This is rare, but could happen.
state = CollisionState.Separated;
return(false);
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity.
// System.Numerics.Vector3 rayCastDirection;
// float lengthSquared = localDirection.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3Ex.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (System.Numerics.Vector3.Normalize(localDirection) + System.Numerics.Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// }
// else
// {
// contact.PenetrationDepth = float.MaxValue;
// contact.Normal = Toolbox.UpVector;
// }
// //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity.
// //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization).
// System.Numerics.Vector3 normalCandidate;
// float depthCandidate;
// lengthSquared = localTransformB.Position.LengthSquared();
// if (lengthSquared > Toolbox.Epsilon)
// {
// Vector3Ex.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate);
// if (depthCandidate < contact.PenetrationDepth)
// {
// contact.Normal = normalCandidate;
// }
// }
// //Correct the penetration depth.
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// ////The local casting can optionally continue. Eventually, it will converge to the local minimum.
// //while (true)
// //{
// // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate);
// // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon)
// // break;
// // contact.PenetrationDepth = depthCandidate;
// // contact.Normal = normalCandidate;
// //}
// contact.Id = -1;
// //we're still in local space! transform it all back.
// Matrix3X3 orientation;
// Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation);
// Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
// //Vector3Ex.Negate(ref contact.Normal, out contact.Normal);
// Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position);
// Vector3Ex.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position);
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact;
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//contact = new ContactData();
//return false;
#endregion
#region Testing
//RigidTransform localTransformB;
//MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB);
//contact.Id = -1;
//if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position))
//{
// System.Numerics.Vector3 rayCastDirection = localTransformB.Position;
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal);
// MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection);
// RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position);
// System.Numerics.Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal);
// return true;
//}
//contact.Normal = new System.Numerics.Vector3();
//contact.PenetrationDepth = 0;
//return false;
#endregion
#region v0.15.2 and before
//if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact))
//{
// if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin)
// state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method.
// return true;
//}
////This is rare, but could happen.
//state = CollisionState.Separated;
//return false;
#endregion
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
System.Numerics.Vector3 axisA, axisB;
QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
System.Numerics.Vector3 twistMeasureAxisA, twistMeasureAxisB;
QuaternionEx.Transform(ref LocalMeasurementAxisA, ref ConnectionA.Orientation, out twistMeasureAxisA);
QuaternionEx.Transform(ref LocalMeasurementAxisB, ref ConnectionB.Orientation, out twistMeasureAxisB);
//Compute the shortest rotation to bring axisB into alignment with axisA.
System.Numerics.Quaternion alignmentRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisB, ref axisA, out alignmentRotation);
//Transform the measurement axis on B by the alignment quaternion.
QuaternionEx.Transform(ref twistMeasureAxisB, ref alignmentRotation, out twistMeasureAxisB);
//We can now compare the angle between the twist axes.
float angle;
Vector3Ex.Dot(ref twistMeasureAxisA, ref twistMeasureAxisB, out angle);
angle = (float)Math.Acos(MathHelper.Clamp(angle, -1, 1));
//Compute the bias based upon the error.
if (angle > maximumAngle)
{
velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * (angle - maximumAngle), 0, 0);
}
else //If the constraint isn't violated, set up the velocity bias to allow a 'speculative' limit.
{
velocityBias = new System.Numerics.Vector3(angle - maximumAngle, 0, 0);
}
//We can't just use the axes directly as jacobians. Consider 'cranking' one object around the other.
System.Numerics.Vector3 jacobian;
Vector3Ex.Add(ref axisA, ref axisB, out jacobian);
float lengthSquared = jacobian.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref jacobian, (float)Math.Sqrt(lengthSquared), out jacobian);
}
else
{
//The constraint is in an invalid configuration. Just ignore it.
jacobian = new System.Numerics.Vector3();
}
//In addition to the absolute angle value, we need to know which side of the limit we're hitting.
//The jacobian will be negated on one side. This is because limits can only 'push' in one direction;
//if we didn't flip the direction of the jacobian, it would be trying to push the same direction on both ends of the limit.
//One side would end up doing nothing!
System.Numerics.Vector3 cross;
Vector3Ex.Cross(ref twistMeasureAxisA, ref twistMeasureAxisB, out cross);
float limitSide;
Vector3Ex.Dot(ref cross, ref axisA, out limitSide);
//Negate the jacobian based on what side of the limit we're on.
if (limitSide < 0)
{
Vector3Ex.Negate(ref jacobian, out jacobian);
}
angularJacobianA = new Matrix3x3 {
M11 = jacobian.X, M12 = jacobian.Y, M13 = jacobian.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -jacobian.X, M12 = -jacobian.Y, M13 = -jacobian.Z
};
}
public static SharpDX.Vector3 AsSharpDX(this System.Numerics.Vector3 vector3)
{
return(new SharpDX.Vector3(vector3.X, vector3.Y, vector3.Z));
}
/// <summary>
/// Constructs a new plane.
/// </summary>
/// <param name="normal">Normal of the plane.</param>
/// <param name="d">Negative distance to the plane from the origin along the normal</param>
public Plane(System.Numerics.Vector3 normal, float d)
{
this.Normal = normal;
this.D = d;
}
/// <summary>
/// Gets the dot product of the position offset from the plane along the plane's normal.
/// </summary>
/// <param name="v">Position to compute the dot product of.</param>
/// <param name="dot">Dot product.</param>
public void DotCoordinate(ref System.Numerics.Vector3 v, out float dot)
{
dot = Normal.X * v.X + Normal.Y * v.Y + Normal.Z * v.Z + D;
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="freeAxis">Axis around which the hinge can rotate.</param>
public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
Limit = new RevoluteLimit(connectionA, connectionB);
Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
Limit.IsActive = false;
Motor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
{
baseAxis = connectionB.position - anchor;
}
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
}
}
Limit.TestAxis = baseAxis;
Motor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Orients the root game object, such that the Scene Understanding floor lies on the Unity world's X-Z plane.
/// </summary>
/// <param name="sceneRoot">Root game object.</param>
/// <param name="scene">Scene Understanding scene.</param>
public void OrientSceneRootForPC(GameObject sceneRoot, SceneUnderstanding.Scene scene)
{
if (scene == null)
{
Logger.LogWarning("SceneUnderstandingUtils.OrientSceneRootForPC: Scene is null.");
return;
}
IEnumerable <SceneUnderstanding.SceneObject> sceneObjects = scene.SceneObjects;
float areaForlargestFloorSoFar = 0;
SceneUnderstanding.SceneObject floorSceneObject = null;
SceneUnderstanding.SceneQuad floorQuad = null;
// Find the largest floor quad.
foreach (SceneUnderstanding.SceneObject so in sceneObjects)
{
if (so.Kind == SceneUnderstanding.SceneObjectKind.Floor)
{
IEnumerable <SceneUnderstanding.SceneQuad> quads = so.Quads;
if (quads != null)
{
foreach (SceneUnderstanding.SceneQuad quad in quads)
{
float quadArea = quad.Extents.X * quad.Extents.Y;
if (quadArea > areaForlargestFloorSoFar)
{
areaForlargestFloorSoFar = quadArea;
floorSceneObject = so;
floorQuad = quad;
}
}
}
}
}
if (floorQuad != null)
{
// Compute the floor quad's normal.
float widthInMeters = floorQuad.Extents.X;
float heightInMeters = floorQuad.Extents.Y;
System.Numerics.Vector3 point1 = new System.Numerics.Vector3(-widthInMeters / 2, -heightInMeters / 2, 0);
System.Numerics.Vector3 point2 = new System.Numerics.Vector3(widthInMeters / 2, -heightInMeters / 2, 0);
System.Numerics.Vector3 point3 = new System.Numerics.Vector3(-widthInMeters / 2, heightInMeters / 2, 0);
System.Numerics.Matrix4x4 floorTransform = floorSceneObject.GetLocationAsMatrix();
floorTransform = TransformUtils.ConvertRightHandedMatrix4x4ToLeftHanded(floorTransform);
System.Numerics.Vector3 tPoint1 = System.Numerics.Vector3.Transform(point1, floorTransform);
System.Numerics.Vector3 tPoint2 = System.Numerics.Vector3.Transform(point2, floorTransform);
System.Numerics.Vector3 tPoint3 = System.Numerics.Vector3.Transform(point3, floorTransform);
System.Numerics.Vector3 p21 = tPoint2 - tPoint1;
System.Numerics.Vector3 p31 = tPoint3 - tPoint1;
System.Numerics.Vector3 floorNormal = System.Numerics.Vector3.Cross(p31, p21);
// Numerics to Unity conversion.
Vector3 floorNormalUnity = new Vector3(floorNormal.X, floorNormal.Y, floorNormal.Z);
// Get the rotation between the floor normal and Unity world's up vector.
Quaternion rotation = Quaternion.FromToRotation(floorNormalUnity, Vector3.up);
// Apply the rotation to the root, so that the floor is on the Camera's x-z plane.
sceneRoot.transform.rotation = rotation;
}
}
