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); }