// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, sfloat tau, sfloat damping)
{
// 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);
sfloat sinAngle = math.length(math.cross(axisAinB, AxisBinB));
sfloat cosAngle = math.dot(axisAinB, AxisBinB);
sfloat angle = math.atan2(sinAngle, cosAngle);
InitialError = JacobianUtilities.CalculateError(angle, MinAngle, MaxAngle);
}
}
internal static void ExecuteImpl(int i, NativeArray <MotionData> motionDatas, NativeArray <MotionVelocity> motionVelocities, sfloat timeStep)
{
MotionData motionData = motionDatas[i];
MotionVelocity motionVelocity = motionVelocities[i];
// Update motion space
{
// center of mass
IntegratePosition(ref motionData.WorldFromMotion.pos, motionVelocity.LinearVelocity, timeStep);
// orientation
IntegrateOrientation(ref motionData.WorldFromMotion.rot, motionVelocity.AngularVelocity, timeStep);
}
// Update velocities
{
// damping
motionVelocity.LinearVelocity *= math.clamp(sfloat.One - motionData.LinearDamping * timeStep, sfloat.Zero, sfloat.One);
motionVelocity.AngularVelocity *= math.clamp(sfloat.One - motionData.AngularDamping * timeStep, sfloat.Zero, sfloat.One);
}
// 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, sfloat tau, sfloat damping)
{
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), sfloat.Zero, 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);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, sfloat tau, sfloat 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);
sfloat initialAngle = math.asin(math.length(jointOrientation.value.xyz)) * (sfloat)2.0f;
InitialError = JacobianUtilities.CalculateError(initialAngle, MinAngle, MaxAngle);
}
// Build the Jacobian
public void Build(
MTransform aFromConstraint, MTransform bFromConstraint,
MotionVelocity velocityA, MotionVelocity velocityB,
MotionData motionA, MotionData motionB,
Constraint constraint, sfloat tau, sfloat 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);
}
