public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedErrorColor, 0);
}
/// <summary>
/// Integrates the position and orientation of the bone forward based upon the current linear and angular velocity.
/// </summary>
internal void UpdatePosition()
{
//Update the position based on the linear velocity.
Vector3Ex.Add(ref Position, ref linearVelocity, out Position);
//Update the orientation based on the angular velocity.
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref angularVelocity, .5f, out increment);
var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
QuaternionEx.Multiply(ref multiplier, ref Orientation, out multiplier);
QuaternionEx.Add(ref Orientation, ref multiplier, out Orientation);
Orientation = System.Numerics.Quaternion.Normalize(Orientation);
//Eliminate any latent velocity in the bone to prevent unwanted simulation feedback.
//This is the only thing conceptually separating this "IK" solver from the regular dynamics loop in BEPUphysics.
//(Well, that and the whole lack of collision detection...)
linearVelocity = new System.Numerics.Vector3();
angularVelocity = new System.Numerics.Vector3();
//Note: Unlike a regular dynamics simulation, we do not include any 'dt' parameter in the above integration.
//Setting the velocity to 0 every update means that no more than a single iteration's worth of velocity accumulates.
//Since the softness of constraints already varies with the time step and bones never accelerate for more than one frame,
//scaling the velocity for position integration actually turns out generally worse.
//This is not a rigorously justifiable approach, but this isn't a regular dynamic simulation anyway.
}
/// <summary>
/// Updates the position of the detector before each step.
/// </summary>
protected internal override void UpdateDetectorPosition()
{
#if !WINDOWS
System.Numerics.Vector3 newPosition = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 newPosition;
#endif
newPosition.X = wheel.suspension.worldAttachmentPoint.X + wheel.suspension.worldDirection.X * wheel.suspension.restLength * .5f;
newPosition.Y = wheel.suspension.worldAttachmentPoint.Y + wheel.suspension.worldDirection.Y * wheel.suspension.restLength * .5f;
newPosition.Z = wheel.suspension.worldAttachmentPoint.Z + wheel.suspension.worldDirection.Z * wheel.suspension.restLength * .5f;
detector.Position = newPosition;
if (IncludeSteeringTransformInCast)
{
System.Numerics.Quaternion localSteeringTransform;
QuaternionEx.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform);
detector.Orientation = QuaternionEx.Concatenate(localSteeringTransform, wheel.Vehicle.Body.orientation);
}
else
{
detector.Orientation = wheel.Vehicle.Body.orientation;
}
System.Numerics.Vector3 linearVelocity;
Vector3Ex.Subtract(ref newPosition, ref wheel.vehicle.Body.position, out linearVelocity);
Vector3Ex.Cross(ref linearVelocity, ref wheel.vehicle.Body.angularVelocity, out linearVelocity);
Vector3Ex.Add(ref linearVelocity, ref wheel.vehicle.Body.linearVelocity, out linearVelocity);
detector.LinearVelocity = linearVelocity;
detector.AngularVelocity = wheel.vehicle.Body.angularVelocity;
}
static void SplitReposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
System.Numerics.Vector3 weightedA, weightedB;
Vector3Ex.Multiply(ref distributionInfoA.Center, weightA, out weightedA);
Vector3Ex.Multiply(ref distributionInfoB.Center, weightB, out weightedB);
System.Numerics.Vector3 newLocalCenter;
Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3Ex.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter);
System.Numerics.Vector3 localOffsetA;
System.Numerics.Vector3 localOffsetB;
Vector3Ex.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA);
Vector3Ex.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB);
System.Numerics.Vector3 originalPosition = a.position;
b.Orientation = a.Orientation;
System.Numerics.Vector3 offsetA = QuaternionEx.Transform(localOffsetA, a.Orientation);
System.Numerics.Vector3 offsetB = QuaternionEx.Transform(localOffsetB, a.Orientation);
a.Position = originalPosition + offsetA;
b.Position = originalPosition + offsetB;
System.Numerics.Vector3 originalLinearVelocity = a.linearVelocity;
System.Numerics.Vector3 originalAngularVelocity = a.angularVelocity;
a.AngularVelocity = originalAngularVelocity;
b.AngularVelocity = originalAngularVelocity;
a.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetA);
b.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetB);
}
///<summary>
/// Concatenates a rigid transform with another rigid transform.
///</summary>
///<param name="a">The first rigid transform.</param>
///<param name="b">The second rigid transform.</param>
///<param name="combined">Concatenated rigid transform.</param>
public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined)
{
System.Numerics.Vector3 intermediate;
QuaternionEx.Transform(ref a.Position, ref b.Orientation, out intermediate);
Vector3Ex.Add(ref intermediate, ref b.Position, out combined.Position);
QuaternionEx.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation);
}
static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref System.Numerics.Vector3 removedCenter)
{
//The compounds are not aligned with the original's position yet.
//In order to align them, first look at the centers the split method computed.
//They are offsets from the center of the original shape in local space.
//These can be used to reposition the objects in world space.
System.Numerics.Vector3 weightedA, weightedB;
Vector3Ex.Multiply(ref distributionInfo.Center, weight, out weightedA);
Vector3Ex.Multiply(ref removedCenter, removedWeight, out weightedB);
System.Numerics.Vector3 newLocalCenter;
Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter);
Vector3Ex.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter);
System.Numerics.Vector3 localOffset;
Vector3Ex.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset);
System.Numerics.Vector3 originalPosition = compound.position;
System.Numerics.Vector3 offset = QuaternionEx.Transform(localOffset, compound.orientation);
compound.Position = originalPosition + offset;
System.Numerics.Vector3 originalLinearVelocity = compound.linearVelocity;
System.Numerics.Vector3 originalAngularVelocity = compound.angularVelocity;
compound.AngularVelocity = originalAngularVelocity;
compound.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offset);
}
private void Initialize()
{
//Compute a vector which is perpendicular to the axis. It'll be added in local space to both connections.
System.Numerics.Vector3 yAxis;
Vector3Ex.Cross(ref worldAxisA, ref Toolbox.UpVector, out yAxis);
float length = yAxis.LengthSquared();
if (length < Toolbox.Epsilon)
{
Vector3Ex.Cross(ref worldAxisA, ref Toolbox.RightVector, out yAxis);
}
yAxis.Normalize();
//Put the axis into the local space of A.
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref connectionA.orientation, out conjugate);
QuaternionEx.Transform(ref yAxis, ref conjugate, out aLocalAxisY);
//Complete A's basis.
Vector3Ex.Cross(ref localAxisA, ref aLocalAxisY, out aLocalAxisZ);
//Rotate the axis to B since it could be arbitrarily rotated.
System.Numerics.Quaternion rotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldAxisA, ref worldAxisB, out rotation);
QuaternionEx.Transform(ref yAxis, ref rotation, out yAxis);
//Put it into local space.
QuaternionEx.Conjugate(ref connectionB.orientation, out conjugate);
QuaternionEx.Transform(ref yAxis, ref conjugate, out bLocalAxisY);
}
public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localDirection, poseA.Orientation, out var worldDirection);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var closestPointOnLine = Vector3.Dot(anchorB - anchorA, worldDirection) * worldDirection + anchorA;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnLine, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(closestPointOnLine, anchorB, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
}
void Process(float t)
{
float pct = MathHelper.Lerp(Event.StartPercent, Event.StopPercent, t);
var path = Path.Entity.Path;
var mat = Path.Rotate * Matrix4x4.CreateTranslation(Path.Translate);
var pos = Vector3.Transform(path.GetPosition(pct) + Event.Offset, mat);
if ((Event.Flags & AttachFlags.LookAt) == AttachFlags.LookAt)
{
var orient = Matrix4x4.CreateFromQuaternion(QuaternionEx.LookRotation(path.GetDirection(pct, Event.StartPercent > Event.StopPercent), Vector3.UnitY)) * Path.Rotate;
Object.Rotate = orient;
if ((Event.Flags & AttachFlags.Position) == AttachFlags.Position)
{
Object.Translate = pos;
}
}
else if ((Event.Flags & AttachFlags.Orientation) == AttachFlags.Orientation)
{
var orient = Path.Rotate * Matrix4x4.CreateFromQuaternion(path.GetOrientation(pct));
Object.Rotate = orient;
if ((Event.Flags & AttachFlags.Position) == AttachFlags.Position)
{
Object.Translate = pos;
}
}
else if ((Event.Flags & AttachFlags.Position) == AttachFlags.Position)
{
Object.Translate = pos;
}
}
public void Swipe(double degree, Vector3 dir)
{
if (isInit())
{
Player.simulator.Simulation.Awakener.AwakenBody(golfBallRef.Handle);
float force = 30;
QuixConsole.Log("Swipé log", degree);
Vector2 rot2d = Player.GetXYRotation();
Vector3 rot = new Vector3(rot2d.X, 0, rot2d.Y);
/* golfBallRef.Velocity.Linear.Y += force;
* golfBallRef.Velocity.Linear.X -= rot.X*force;
* golfBallRef.Velocity.Linear.Z -= rot.Y*force;*/
Quaternion quat = QuaternionEx.CreateFromAxisAngle(rot, (float)degree);
Matrix4x4 matrix = Matrix4x4.CreateFromQuaternion(quat);
Vector3 result = Vector3.Transform(rot, matrix);
QuixConsole.Log("direction", dir);
dir.X *= -1;
dir.Y *= -1;
dir.Z *= -1;
golfBallRef.Velocity.Linear.Y += dir.Y * force;
golfBallRef.Velocity.Linear.X += dir.X * force;
golfBallRef.Velocity.Linear.Z += dir.Z * force;
QuixConsole.Log("Result", result);
}
}
public override void Update(Window window, Camera camera, Input input, float dt)
{
if (input.WasPushed(OpenTK.Input.Key.Z))
{
//INVEST TODAY FOR INCREDIBLE RETURNS DON'T MISS OUT LOOK AT THE COINS THERE ARE A LOT OF THEM AND THEY COULD BE YOURS
var origin = new Vector3(-30, 5, -30) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(60, 30, 60);
for (int i = 0; i < 128; ++i)
{
var direction = new Vector3(-1) + 2 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
var length = direction.Length();
if (length > 1e-7f)
{
direction /= length;
}
else
{
direction = new Vector3(0, 1, 0);
}
coinDescription.Pose.Position = origin + direction * 10 * (float)random.NextDouble();
coinDescription.Pose.Orientation = QuaternionEx.Normalize(new Quaternion(0.01f + (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()));
coinDescription.Velocity.Linear = direction * (5 + 30 * (float)random.NextDouble());
Simulation.Bodies.Add(coinDescription);
}
}
base.Update(window, camera, input, dt);
}
/// <summary>
/// Automatically computes the measurement axes for the current local axes.
/// The current relative state of the entities will be considered 0 twist angle.
/// </summary>
public void ComputeMeasurementAxes()
{
System.Numerics.Vector3 axisA, axisB;
QuaternionEx.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
QuaternionEx.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
//Pick an axis perpendicular to axisA to use as the measurement axis.
System.Numerics.Vector3 worldMeasurementAxisA;
Vector3Ex.Cross(ref Toolbox.UpVector, ref axisA, out worldMeasurementAxisA);
float lengthSquared = worldMeasurementAxisA.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3Ex.Divide(ref worldMeasurementAxisA, (float)Math.Sqrt(lengthSquared), out worldMeasurementAxisA);
}
else
{
//Oops! It was parallel to the up vector. Just try again with the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref axisA, out worldMeasurementAxisA);
worldMeasurementAxisA.Normalize();
}
//Attach the measurement axis to entity B.
//'Push' A's axis onto B by taking into account the swing transform.
System.Numerics.Quaternion alignmentRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref axisA, ref axisB, out alignmentRotation);
System.Numerics.Vector3 worldMeasurementAxisB;
QuaternionEx.Transform(ref worldMeasurementAxisA, ref alignmentRotation, out worldMeasurementAxisB);
//Plop them on!
MeasurementAxisA = worldMeasurementAxisA;
MeasurementAxisB = worldMeasurementAxisB;
}
public bool DropToGround(Vector3 targetPos, bool force = false)
{
float range = (force ? 10000f : (maxStepHeight + maxStepDownHeight));
Vector3 pos;
Vector3 normal;
if (TransformUtil.GetGroundInfo(targetPos, out pos, out normal, range, 278986753))
{
if (UnityEngine.Physics.CheckSphere(pos + Vector3.up * 1f, 0.2f, 278986753))
{
return(false);
}
base.transform.position = pos;
Vector3 eulerAngles = QuaternionEx.LookRotationForcedUp(base.transform.forward, averagedUp).eulerAngles;
if (eulerAngles.z > 180f)
{
eulerAngles.z -= 360f;
}
else if (eulerAngles.z < -180f)
{
eulerAngles.z += 360f;
}
eulerAngles.z = Mathf.Clamp(eulerAngles.z, -10f, 10f);
base.transform.rotation = Quaternion.Euler(eulerAngles);
return(true);
}
return(false);
}
/// <summary>
/// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases.
/// </summary>
/// <param name="worldTwistAxisA">Twist axis in world space to attach to entity A.</param>
/// <param name="worldTwistAxisB">Twist axis in world space to attach to entity B.</param>
public void SetupJointTransforms(System.Numerics.Vector3 worldTwistAxisA, System.Numerics.Vector3 worldTwistAxisB)
{
worldTwistAxisA.Normalize();
worldTwistAxisB.Normalize();
System.Numerics.Vector3 worldXAxis;
Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis);
float length = worldXAxis.LengthSquared();
if (length < Toolbox.Epsilon)
{
Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis);
}
worldXAxis.Normalize();
//Complete A's basis.
System.Numerics.Vector3 worldYAxis;
Vector3Ex.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis);
basisA.rotationMatrix = connectionA.orientationMatrix;
basisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis);
//Rotate the axis to B since it could be arbitrarily rotated.
System.Numerics.Quaternion rotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation);
QuaternionEx.Transform(ref worldXAxis, ref rotation, out worldXAxis);
basisB.rotationMatrix = connectionB.orientationMatrix;
basisB.SetWorldAxes(worldTwistAxisB, worldXAxis);
}
public void Test(Random random, int innerIterations)
{
GenerateRandomNormalizedVector(random, out var v1);
GenerateRandomNormalizedVector(random, out var v2);
for (int i = 0; i < innerIterations; ++i)
{
QuaternionEx.GetQuaternionBetweenNormalizedVectors(v1, v2, out var v1ToV2);
QuaternionEx.GetQuaternionBetweenNormalizedVectors(v2, v1, out var v2ToV1);
#if DEBUG
QuaternionEx.ConcatenateWithoutOverlap(v1ToV2, v2ToV1, out var concatenated);
QuaternionEx.Transform(v1, v1ToV2, out var v1TransformedToV2);
QuaternionEx.Transform(v2, v2ToV1, out var v2TransformedToV1);
QuaternionEx.Transform(v1, concatenated, out var v1TransformedToV1);
var v1ToV2ErrorLength = (v1TransformedToV2 - v2).LengthSquared();
var v2ToV1ErrorLength = (v2TransformedToV1 - v1).LengthSquared();
var v1ToV1ErrorLength = (v1TransformedToV1 - v1).LengthSquared();
const float epsilon = 1e-6f;
Debug.Assert(
v1ToV2ErrorLength < epsilon &&
v2ToV1ErrorLength < epsilon &&
v1ToV1ErrorLength < epsilon);
#endif
}
}
void IPositionUpdateable.PreUpdatePosition(float dt)
{
System.Numerics.Vector3 increment;
Vector3Ex.Multiply(ref angularVelocity, dt * .5f, out increment);
var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
QuaternionEx.Multiply(ref multiplier, ref orientation, out multiplier);
QuaternionEx.Add(ref orientation, ref multiplier, out orientation);
orientation = System.Numerics.Quaternion.Normalize(orientation);
Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);
//Only do the linear motion if this object doesn't obey CCD.
if (PositionUpdateMode == PositionUpdateMode.Discrete)
{
Vector3Ex.Multiply(ref linearVelocity, dt, out increment);
Vector3Ex.Add(ref position, ref increment, out position);
collisionInformation.UpdateWorldTransform(ref position, ref orientation);
//The position update is complete if this is a discretely updated object.
if (PositionUpdated != null)
{
PositionUpdated(this);
}
}
MathChecker.Validate(linearVelocity);
MathChecker.Validate(angularVelocity);
MathChecker.Validate(position);
MathChecker.Validate(orientation);
#if CONSERVE
MathChecker.Validate(angularMomentum);
#endif
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobian = new Matrix3x3();
System.Numerics.Vector3 boneAxis;
QuaternionEx.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis);
angularJacobian = new Matrix3x3
{
M11 = constrainedAxis1.X,
M12 = constrainedAxis1.Y,
M13 = constrainedAxis1.Z,
M21 = constrainedAxis2.X,
M22 = constrainedAxis2.Y,
M23 = constrainedAxis2.Z
};
System.Numerics.Vector3 error;
Vector3Ex.Cross(ref boneAxis, ref freeAxis, out error);
System.Numerics.Vector2 constraintSpaceError;
Vector3Ex.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X);
Vector3Ex.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y);
velocityBias.X = errorCorrectionFactor * constraintSpaceError.X;
velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = Matrix3x3.Identity;
//The jacobian entries are is [ La, Aa, -Lb, -Ab ] because the relative velocity is computed using A-B. So, negate B's jacobians!
linearJacobianB = new Matrix3x3 {
M11 = -1, M22 = -1, M33 = -1
};
System.Numerics.Vector3 rA;
QuaternionEx.Transform(ref LocalOffsetA, ref ConnectionA.Orientation, out rA);
Matrix3x3.CreateCrossProduct(ref rA, out angularJacobianA);
//Transposing a skew-symmetric matrix is equivalent to negating it.
Matrix3x3.Transpose(ref angularJacobianA, out angularJacobianA);
System.Numerics.Vector3 worldPositionA;
Vector3Ex.Add(ref ConnectionA.Position, ref rA, out worldPositionA);
System.Numerics.Vector3 rB;
QuaternionEx.Transform(ref LocalOffsetB, ref ConnectionB.Orientation, out rB);
Matrix3x3.CreateCrossProduct(ref rB, out angularJacobianB);
System.Numerics.Vector3 worldPositionB;
Vector3Ex.Add(ref ConnectionB.Position, ref rB, out worldPositionB);
System.Numerics.Vector3 linearError;
Vector3Ex.Subtract(ref worldPositionB, ref worldPositionA, out linearError);
Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias);
}
public override void Apply(ref Vector3 pos, ref Quaternion rot, ref Vector3 scale)
{
Vector3 normal = TerrainMeta.HeightMap.GetNormal(pos);
Vector3 vector = ((normal == Vector3.up) ? Vector3.forward : Vector3.Cross(normal, Vector3.up));
Vector3 vector2 = Vector3.Cross(normal, vector);
if (SlopeOffset != Vector3.zero || SlopeScale != Vector3.one)
{
float slope = TerrainMeta.HeightMap.GetSlope01(pos);
if (SlopeOffset != Vector3.zero)
{
Vector3 vector3 = SlopeOffset * slope;
pos += vector3.x * vector;
pos += vector3.y * normal;
pos -= vector3.z * vector2;
}
if (SlopeScale != Vector3.one)
{
Vector3 vector4 = Vector3.Lerp(Vector3.one, Vector3.one + Quaternion.Inverse(rot) * (SlopeScale - Vector3.one), slope);
scale.x *= vector4.x;
scale.y *= vector4.y;
scale.z *= vector4.z;
}
}
Vector3 up = Vector3.Lerp(rot * Vector3.up, normal, NormalAlignment);
Quaternion quaternion = QuaternionEx.LookRotationForcedUp(Vector3.Lerp(rot * Vector3.forward, vector2, GradientAlignment), up);
rot = quaternion * rot;
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
angularJacobianA = new Matrix3x3 {
M11 = 1, M22 = 1, M33 = 1
};
angularJacobianB = new Matrix3x3 {
M11 = -1, M22 = -1, M33 = -1
};
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
System.Numerics.Quaternion bTarget;
QuaternionEx.Concatenate(ref GoalRelativeOrientation, ref ConnectionA.Orientation, out bTarget);
System.Numerics.Quaternion bTargetConjugate;
QuaternionEx.Conjugate(ref bTarget, out bTargetConjugate);
System.Numerics.Quaternion error;
QuaternionEx.Concatenate(ref bTargetConjugate, ref ConnectionB.Orientation, out error);
//Convert the error into an axis-angle vector usable for bias velocity.
float angle;
System.Numerics.Vector3 axis;
QuaternionEx.GetAxisAngleFromQuaternion(ref error, out axis, out angle);
velocityBias.X = errorCorrectionFactor * axis.X * angle;
velocityBias.Y = errorCorrectionFactor * axis.Y * angle;
velocityBias.Z = errorCorrectionFactor * axis.Z * angle;
}
public override void Apply(ref Vector3 pos, ref Quaternion rot, ref Vector3 scale)
{
Vector3 normal = TerrainMeta.HeightMap.GetNormal(pos);
Vector3 vector3 = (normal == Vector3.up ? Vector3.forward : Vector3.Cross(normal, Vector3.up));
Vector3 vector31 = Vector3.Cross(normal, vector3);
if (this.SlopeOffset != Vector3.zero || this.SlopeScale != Vector3.one)
{
float slope01 = TerrainMeta.HeightMap.GetSlope01(pos);
if (this.SlopeOffset != Vector3.zero)
{
Vector3 slopeOffset = this.SlopeOffset * slope01;
pos = pos + (slopeOffset.x * vector3);
pos = pos + (slopeOffset.y * normal);
pos = pos - (slopeOffset.z * vector31);
}
if (this.SlopeScale != Vector3.one)
{
Vector3 vector32 = Vector3.Lerp(Vector3.one, Vector3.one + (Quaternion.Inverse(rot) * (this.SlopeScale - Vector3.one)), slope01);
scale.x *= vector32.x;
scale.y *= vector32.y;
scale.z *= vector32.z;
}
}
Vector3 vector33 = Vector3.Lerp(rot * Vector3.up, normal, this.NormalAlignment);
Quaternion quaternion = QuaternionEx.LookRotationForcedUp(Vector3.Lerp(rot * Vector3.forward, vector31, this.GradientAlignment), vector33);
rot = quaternion * rot;
}
///<summary>
/// Updates the world transform of the shape using the given position and orientation.
/// The world transform of the shape is offset from the given position and orientation by the collidable's LocalPosition.
///</summary>
///<param name="position">Position to use for the calculation.</param>
///<param name="orientation">Orientation to use for the calculation.</param>
public virtual void UpdateWorldTransform(ref System.Numerics.Vector3 position, ref System.Numerics.Quaternion orientation)
{
QuaternionEx.Transform(ref localPosition, ref orientation, out worldTransform.Position);
Vector3Ex.Add(ref worldTransform.Position, ref position, out worldTransform.Position);
worldTransform.Orientation = orientation;
worldTransform.Validate();
}
/// <summary>
/// Constructs a 3DOF angular joint which tries to keep two bones in angular alignment.
/// </summary>
/// <param name="connectionA">First bone to connect to the joint.</param>
/// <param name="connectionB">Second bone to connect to the joint.</param>
public IKAngularJoint(Bone connectionA, Bone connectionB)
: base(connectionA, connectionB)
{
System.Numerics.Quaternion orientationAConjugate;
QuaternionEx.Conjugate(ref ConnectionA.Orientation, out orientationAConjugate);
//Store the orientation from A to B in A's local space in the GoalRelativeOrientation.
QuaternionEx.Concatenate(ref ConnectionB.Orientation, ref orientationAConjugate, out GoalRelativeOrientation);
}
///<summary>
/// Transforms a position by a rigid transform's inverse.
///</summary>
///<param name="position">Position to transform.</param>
///<param name="transform">Transform to invert and apply.</param>
///<param name="result">Transformed position.</param>
public static void TransformByInverse(ref System.Numerics.Vector3 position, ref RigidTransform transform, out System.Numerics.Vector3 result)
{
System.Numerics.Quaternion orientation;
System.Numerics.Vector3 intermediate;
Vector3Ex.Subtract(ref position, ref transform.Position, out intermediate);
QuaternionEx.Conjugate(ref transform.Orientation, out orientation);
QuaternionEx.Transform(ref intermediate, ref orientation, out result);
}
/// <summary>
/// Constructs a new constraint which prevents relative angular motion between the two connected bodies.
/// </summary>
/// <param name="connectionA">First connection of the pair.</param>
/// <param name="connectionB">Second connection of the pair.</param>
public NoRotationJoint(Entity connectionA, Entity connectionB)
{
ConnectionA = connectionA;
ConnectionB = connectionB;
initialQuaternionConjugateA = QuaternionEx.Conjugate(ConnectionA.orientation);
initialQuaternionConjugateB = QuaternionEx.Conjugate(ConnectionB.orientation);
}
static void CreateBallSocket(ref RigidPose a, ref RigidPose b, out BallSocket description)
{
var midpoint = 0.5f * (a.Position + b.Position);
description.LocalOffsetA = QuaternionEx.Transform(midpoint - a.Position, QuaternionEx.Conjugate(a.Orientation));
description.LocalOffsetB = QuaternionEx.Transform(midpoint - b.Position, QuaternionEx.Conjugate(b.Orientation));
description.SpringSettings = new SpringSettings(15, 0.1f);
}
public float ObstacleDistanceCheck(float speed = 15f)
{
RaycastHit raycastHit;
Vector3 vector3 = base.transform.position;
int num = 1;
int num1 = Mathf.CeilToInt((float)(Mathf.Max(2, Mathf.CeilToInt(speed)) / num));
float single = 0f;
Vector3 vector31 = QuaternionEx.LookRotationForcedUp(base.transform.forward, Vector3.up) * Vector3.forward;
Vector3 vector32 = this.movementLOSOrigin.transform.position;
vector32.y = base.transform.position.y;
Vector3 vector33 = base.transform.up;
for (int i = 0; i < num1; i++)
{
float single1 = (float)num;
bool flag = false;
float single2 = 0f;
Vector3 vector34 = Vector3.zero;
Vector3 vector35 = Vector3.up;
Vector3 vector36 = vector32 + (Vector3.up * 0.5f);
if (Physics.SphereCast(vector36, this.obstacleDetectionRadius, vector31, out raycastHit, single1, 1486954753))
{
single2 = raycastHit.distance;
vector34 = raycastHit.point;
vector35 = raycastHit.normal;
flag = true;
}
if (!flag)
{
if (!TransformUtil.GetGroundInfo((vector36 + (vector31 * 1f)) + (Vector3.up * 2f), out vector34, out vector35, 4f, 278986753, null))
{
return(single);
}
single2 = Vector3.Distance(vector36, vector34);
if (WaterLevel.Test(vector34 + (Vector3.one * this.maxWaterDepth)))
{
vector35 = -base.transform.forward;
}
flag = true;
}
if (flag)
{
float single3 = Vector3.Angle(vector33, vector35);
float single4 = Vector3.Angle(vector35, Vector3.up);
if (single3 > 45f && single4 > 50f)
{
return(single);
}
}
single += single2;
vector31 = QuaternionEx.LookRotationForcedUp(base.transform.forward, vector35) * Vector3.forward;
vector32 = vector34;
}
return(single);
}
///<summary>
/// Gets the local transform of B in the space of A.
///</summary>
///<param name="transformA">First transform.</param>
///<param name="transformB">Second transform.</param>
///<param name="localTransformB">Transform of B in the local space of A.</param>
public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB,
out RigidTransform localTransformB)
{
//Put B into A's space.
System.Numerics.Quaternion conjugateOrientationA;
QuaternionEx.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
QuaternionEx.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3Ex.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
QuaternionEx.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
}
///<summary>
/// Gets the extreme point of the shape in world space in a given direction.
///</summary>
///<param name="direction">Direction to find the extreme point in.</param>
/// <param name="shapeTransform">Transform to use for the shape.</param>
///<param name="extremePoint">Extreme point on the shape.</param>
public void GetExtremePointWithoutMargin(System.Numerics.Vector3 direction, ref RigidTransform shapeTransform, out System.Numerics.Vector3 extremePoint)
{
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate);
QuaternionEx.Transform(ref direction, ref conjugate, out direction);
GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
QuaternionEx.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint);
Vector3Ex.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint);
}
void UpdateIndex(int i)
{
Entity entity = manager.entities[i];
//Blend between previous and current states.
//Interpolated updates occur after proper updates complete.
//That means that the internal positions and the front buffer positions are equivalent.
//However, the backbuffer is uncontested and contains the previous frame's data.
Vector3Ex.Lerp(ref manager.ReadBuffers.backBuffer[i].Position, ref entity.position, blendAmount, out backBuffer[i].Position);
QuaternionEx.Slerp(ref manager.ReadBuffers.backBuffer[i].Orientation, ref entity.orientation, blendAmount, out backBuffer[i].Orientation);
}
