private BonePartialSolution SolveSingleBone(
RigidBone bone,
Vector3 worldSource, Vector3 worldTarget, MassMoment[] massMoments, Vector3 figureCenterOverride,
RigidBoneSystemInputs inputs, RigidTransform[] boneTransforms)
{
if (bone.Parent == boneSystem.RootBone)
{
//skip the hip bone because it's the same as the root bone but with a different center
return(BonePartialSolution.Zero);
}
var center = bone != boneSystem.RootBone ? boneTransforms[bone.Index].Transform(bone.CenterPoint) : figureCenterOverride;
var parentTotalRotation = bone.Parent != null ? boneTransforms[bone.Parent.Index].Rotation : Quaternion.Identity;
var boneToWorldSpaceRotation = bone.OrientationSpace.Orientation.Chain(parentTotalRotation);
var worldToBoneSpaceRotation = Quaternion.Invert(boneToWorldSpaceRotation);
var boneSpaceSource = Vector3.Transform(worldSource - center, worldToBoneSpaceRotation);
var boneSpaceTarget = Vector3.Transform(worldTarget - center, worldToBoneSpaceRotation);
var force = boneSpaceTarget - boneSpaceSource;
var torque = Vector3.Cross(boneSpaceSource, force);
float mass = boneAttributes[bone.Index].MassIncludingDescendants;
Vector3 unnormalizedAxisOfRotation = Vector3.Cross(worldSource - center, worldTarget - center);
float unnormalizedAxisOfRotationLength = unnormalizedAxisOfRotation.Length();
if (MathUtil.IsZero(unnormalizedAxisOfRotationLength))
{
return(BonePartialSolution.Zero);
}
Vector3 axisOfRotation = unnormalizedAxisOfRotation / unnormalizedAxisOfRotationLength;
float momentOfInertia = massMoments[bone.Index].GetMomentOfInertia(axisOfRotation, center);
var angularVelocity = torque / momentOfInertia;
var twistAxis = bone.RotationOrder.TwistAxis;
var existingRotation = bone.GetOrientedSpaceRotation(inputs).AsQuaternion(twistAxis);
var relaxedRotation = bone.Constraint.Center.AsQuaternion(twistAxis);
float relaxationBias = InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, existingRotation, angularVelocity);
angularVelocity *= relaxationBias;
var linearVelocity = Vector3.Cross(angularVelocity, boneSpaceSource);
var rotation = QuaternionExtensions.RotateBetween(
Vector3.Normalize(boneSpaceSource),
Vector3.Normalize(boneSpaceTarget));
var radius = boneSpaceSource.Length();
var distance = rotation.AccurateAngle() * radius;
float time = distance == 0 ? 0 : distance / linearVelocity.Length();
DebugUtilities.AssertFinite(angularVelocity);
return(new BonePartialSolution {
angularVelocity = angularVelocity,
time = time
});
}
public void SetOrientedSpaceRotation(RigidBoneSystemInputs inputs, TwistSwing orientatedSpaceRotation, bool applyClamp = false)
{
DebugUtilities.AssertFinite(orientatedSpaceRotation);
if (applyClamp)
{
orientatedSpaceRotation = Constraint.Clamp(orientatedSpaceRotation);
}
inputs.Rotations[Index] = orientatedSpaceRotation;
}
public void Dump(string name, UvSet uvSet)
{
DirectoryInfo uvSetDirectory = uvSetsDirectory.Subdirectory(name);
if (uvSetDirectory.Exists)
{
return;
}
Console.WriteLine($"Dumping uv-set {name}...");
int subdivisionLevel = surfaceProperties.SubdivisionLevel;
var geometry = figure.Geometry;
var spatialControlTopology = new QuadTopology(geometry.VertexCount, geometry.Faces);
var spatialControlPositions = geometry.VertexPositions;
QuadTopology spatialTopology;
LimitValues <Vector3> spatialLimitPositions;
using (var refinement = new Refinement(spatialControlTopology, subdivisionLevel)) {
spatialTopology = refinement.GetTopology();
spatialLimitPositions = refinement.LimitFully(spatialControlPositions);
}
uvSet = RemapToDefault(uvSet);
var texturedControlTopology = new QuadTopology(uvSet.Uvs.Length, uvSet.Faces);
Vector2[] controlTextureCoords = uvSet.Uvs;
int[] controlSpatialIdxMap = QuadTopology.CalculateVertexIndexMap(texturedControlTopology, spatialControlTopology.Faces);
Vector3[] texturedControlPositions = controlSpatialIdxMap
.Select(spatialIdx => spatialControlPositions[spatialIdx])
.ToArray();
QuadTopology texturedTopology;
LimitValues <Vector3> texturedLimitPositions;
LimitValues <Vector2> limitTextureCoords;
using (var refinement = new Refinement(texturedControlTopology, surfaceProperties.SubdivisionLevel, BoundaryInterpolation.EdgeAndCorner)) {
texturedTopology = refinement.GetTopology();
texturedLimitPositions = refinement.LimitFully(texturedControlPositions);
limitTextureCoords = refinement.LimitFully(controlTextureCoords);
}
Vector2[] textureCoords;
if (geometry.Type == GeometryType.SubdivisionSurface)
{
textureCoords = limitTextureCoords.values;
}
else
{
if (subdivisionLevel != 0)
{
throw new InvalidOperationException("polygon meshes cannot be subdivided");
}
Debug.Assert(limitTextureCoords.values.Length == controlTextureCoords.Length);
textureCoords = controlTextureCoords;
}
int[] spatialIdxMap = QuadTopology.CalculateVertexIndexMap(texturedTopology, spatialTopology.Faces);
TexturedVertexInfo[] texturedVertexInfos = Enumerable.Range(0, textureCoords.Length)
.Select(idx => {
int spatialVertexIdx = spatialIdxMap[idx];
Vector2 textureCoord = textureCoords[idx];
Vector3 positionDu = TangentSpaceUtilities.CalculatePositionDu(
limitTextureCoords.tangents1[idx],
limitTextureCoords.tangents2[idx],
texturedLimitPositions.tangents1[idx],
texturedLimitPositions.tangents2[idx]);
Vector3 spatialPositionTan1 = spatialLimitPositions.tangents1[spatialVertexIdx];
Vector3 spatialPositionTan2 = spatialLimitPositions.tangents2[spatialVertexIdx];
Vector2 tangentUCoeffs = TangentSpaceUtilities.CalculateTangentSpaceRemappingCoeffs(spatialPositionTan1, spatialPositionTan2, positionDu);
DebugUtilities.AssertFinite(tangentUCoeffs.X);
DebugUtilities.AssertFinite(tangentUCoeffs.Y);
return(new TexturedVertexInfo(
textureCoord,
tangentUCoeffs));
})
.ToArray();
uvSetDirectory.CreateWithParents();
uvSetDirectory.File("textured-vertex-infos.array").WriteArray(texturedVertexInfos);
}
