public void Execute(int i)
{
MotionData motionData = MotionDatas[i];
MotionVelocity motionVelocity = MotionVelocities[i];
// Update motion space
{
// center of mass
motionData.WorldFromMotion.pos += motionVelocity.LinearVelocity * Timestep;
// orientation
IntegrateOrientation(ref motionData.WorldFromMotion.rot, motionVelocity.AngularVelocity, Timestep);
}
// Update velocities
{
// damping
motionVelocity.LinearVelocity *= math.clamp(1.0f - motionData.LinearDamping * Timestep, 0.0f, 1.0f);
motionVelocity.AngularVelocity *= math.clamp(1.0f - motionData.AngularDamping * Timestep, 0.0f, 1.0f);
}
// Write back
MotionDatas[i] = motionData;
MotionVelocities[i] = motionVelocity;
}
internal static void ExecuteImpl(int i, NativeArray <MotionData> motionDatas, NativeArray <MotionVelocity> motionVelocities, float timeStep)
{
MotionData motionData = motionDatas[i];
MotionVelocity motionVelocity = motionVelocities[i];
// Update motion space
{
// center of mass
motionData.WorldFromMotion.pos += motionVelocity.LinearVelocity * timeStep;
// orientation
IntegrateOrientation(ref motionData.WorldFromMotion.rot, motionVelocity.AngularVelocity, timeStep);
}
// Update velocities
{
// damping
motionVelocity.LinearVelocity *= math.clamp(1.0f - motionData.LinearDamping * timeStep, 0.0f, 1.0f);
motionVelocity.AngularVelocity *= math.clamp(1.0f - motionData.AngularDamping * timeStep, 0.0f, 1.0f);
}
// Write back
motionDatas[i] = motionData;
motionVelocities[i] = motionVelocity;
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(AngularLimit2DJacobian);
// Copy the constraint data
int freeIndex = constraint.FreeAxis2D;
AxisAinA = aFromConstraint.Rotation[freeIndex];
AxisBinB = bFromConstraint.Rotation[freeIndex];
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
// Calculate the initial error
{
float3 axisAinB = math.mul(BFromA, AxisAinA);
float sinAngle = math.length(math.cross(axisAinB, AxisBinB));
float cosAngle = math.dot(axisAinB, AxisBinB);
float angle = math.atan2(sinAngle, cosAngle);
InitialError = JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle);
}
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(LinearLimitJacobian);
WorldFromA = motionA.WorldFromMotion;
WorldFromB = motionB.WorldFromMotion;
PivotAinA = aFromConstraint.Translation;
PivotBinB = bFromConstraint.Translation;
AxisInB = float3.zero;
Is1D = false;
MinDistance = constraint.Min;
MaxDistance = constraint.Max;
Tau = tau;
Damping = damping;
// TODO.ma - this code is not always correct in its choice of pivotB.
// The constraint model is asymmetrical. B is the master, and the constraint feature is defined in B-space as a region affixed to body B.
// For example, we can conceive of a 1D constraint as a plane attached to body B through constraint.PivotB, and constraint.PivotA is constrained to that plane.
// A 2D constraint is a line attached to body B. A 3D constraint is a point.
// So, while we always apply an impulse to body A at pivotA, we apply the impulse to body B somewhere on the constraint region.
// This code chooses that point by projecting pivotA onto the point, line or plane, which seems pretty reasonable and also analogous to how contact constraints work.
// However, if the limits are nonzero, then the region is not a point, line or plane. It is a spherical shell, cylindrical shell, or the space between two parallel planes.
// In that case, it is not projecting A to a point on the constraint region. This will not prevent solving the constraint, but the solution may not look correct.
// For now I am leaving it because it is not important to get the most common constraint situations working. If you use a ball and socket, or a prismatic constraint with a
// static master body, or a stiff spring, then there's no problem. However, I think it should eventually be fixed. The min and max limits have different projections, so
// probably the best solution is to make two jacobians whenever min != max. My assumption is that 99% of these are ball and sockets with min = max = 0, so I would rather have
// some waste in the min != max case than generalize this code to deal with different pivots and effective masses depending on which limit is hit.
if (!math.all(constraint.ConstrainedAxes))
{
Is1D = constraint.ConstrainedAxes.x ^ constraint.ConstrainedAxes.y ^ constraint.ConstrainedAxes.z;
// Project pivot A onto the line or plane in B that it is attached to
RigidTransform bFromA = math.mul(math.inverse(WorldFromB), WorldFromA);
float3 pivotAinB = math.transform(bFromA, PivotAinA);
float3 diff = pivotAinB - PivotBinB;
for (int i = 0; i < 3; i++)
{
float3 column = bFromConstraint.Rotation[i];
AxisInB = math.select(column, AxisInB, Is1D ^ constraint.ConstrainedAxes[i]);
float3 dot = math.select(math.dot(column, diff), 0.0f, constraint.ConstrainedAxes[i]);
PivotBinB += column * dot;
}
}
// Calculate the current error
InitialError = CalculateError(
new MTransform(WorldFromA.rot, WorldFromA.pos),
new MTransform(WorldFromB.rot, WorldFromB.pos),
out float3 directionUnused);
}
public void Execute(int i)
{
MotionData motionData = MotionDatas[i];
MotionVelocity motionVelocity = MotionVelocities[i];
// Apply gravity
motionVelocity.LinearVelocity += GravityAcceleration * motionData.GravityFactor;
// Write back
MotionVelocities[i] = motionVelocity;
// Make a copy
InputVelocities[i] = new Velocity
{
Linear = motionVelocity.LinearVelocity,
Angular = motionVelocity.AngularVelocity
};
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
RefBFromA = new quaternion(math.mul(bFromConstraint.Rotation, aFromConstraint.InverseRotation));
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
quaternion jointOrientation = math.mul(math.inverse(RefBFromA), BFromA);
float initialAngle = math.asin(math.length(jointOrientation.value.xyz)) * 2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
internal static void ExecuteImpl(int i, float3 gravityAcceleration,
NativeSlice <MotionData> motionDatas, NativeSlice <MotionVelocity> motionVelocities, NativeSlice <Velocity> inputVelocities)
{
MotionData motionData = motionDatas[i];
MotionVelocity motionVelocity = motionVelocities[i];
// Apply gravity
motionVelocity.LinearVelocity += gravityAcceleration * motionData.GravityFactor;
// Write back
motionVelocities[i] = motionVelocity;
// Make a copy
inputVelocities[i] = new Velocity
{
Linear = motionVelocity.LinearVelocity,
Angular = motionVelocity.AngularVelocity
};
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
// Copy the constraint into the jacobian
AxisIndex = constraint.ConstrainedAxis1D;
AxisInMotionA = aFromConstraint.Rotation[AxisIndex];
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
MotionBFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
MotionAFromJoint = new quaternion(aFromConstraint.Rotation);
MotionBFromJoint = new quaternion(bFromConstraint.Rotation);
// Calculate the current error
InitialError = CalculateError(MotionBFromA);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, float tau, float damping)
{
this = default(AngularLimit3DJacobian);
BFromA = math.mul(math.inverse(motionB.WorldFromMotion.rot), motionA.WorldFromMotion.rot);
MotionAFromJoint = new quaternion(aFromConstraint.Rotation);
MotionBFromJoint = new quaternion(bFromConstraint.Rotation);
MinAngle = constraint.Min;
MaxAngle = constraint.Max;
Tau = tau;
Damping = damping;
float initialAngle = math.atan2(math.length(BFromA.value.xyz), BFromA.value.w) * 2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
// Gets a body's motion, even if the body is static
// TODO - share code with Solver.GetMotions()?
private static void GetMotion(ref PhysicsWorld world, int bodyIndex, out MotionVelocity velocity, out MotionData motion)
{
if (bodyIndex >= world.MotionVelocities.Length)
{
// Body is static
RigidBody body = world.Bodies[bodyIndex];
velocity = MotionVelocity.Zero;
motion = new MotionData
{
WorldFromMotion = body.WorldFromBody,
BodyFromMotion = RigidTransform.identity
// remaining fields all zero
};
}
else
{
// Body is dynamic
velocity = world.MotionVelocities[bodyIndex];
motion = world.MotionDatas[bodyIndex];
}
}
