diff --git a/Common/Model/Animation/ClientAnimator.cs b/Common/Model/Animation/ClientAnimator.cs index 7b695409..a46b42d5 100644 --- a/Common/Model/Animation/ClientAnimator.cs +++ b/Common/Model/Animation/ClientAnimator.cs @@ -52,7 +52,8 @@ public static ClientAnimator CreateForEntity(Entity entity, List ro entity.AnimManager.TriggerAnimationStopped, entity.AnimManager.ShouldPlaySound ); - } else + } + else { return new ClientAnimator( () => 1, @@ -164,9 +165,9 @@ protected virtual void initMatrices(int maxJointId) // Matrices are only required on client side; and from 1.20.5 limited to the MaxJointId (x16) in length to avoid wasting RAM var identMat = ClientAnimator.identMat; var defaultPoseMatrices = new float[16 * maxJointId]; - for (int i = 0; i < defaultPoseMatrices.Length; i++) + for (int i = 0; i < defaultPoseMatrices.Length; i += 16) { - defaultPoseMatrices[i] = identMat[i % 16]; + Buffer.BlockCopy(identMat, 0, defaultPoseMatrices, i * sizeof(float), 16 * sizeof(float)); } TransformationMatricesDefaultPose = defaultPoseMatrices; TransformationMatrices = new float[defaultPoseMatrices.Length]; @@ -189,7 +190,8 @@ protected virtual void LoadAttachmentPoints(List cachedPoses) for (int j = 0; j < attachmentPoints.Length; j++) { AttachmentPoint apoint = attachmentPoints[j]; - AttachmentPointByCode[apoint.Code] = new AttachmentPointAndPose() { + AttachmentPointByCode[apoint.Code] = new AttachmentPointAndPose() + { AttachPoint = apoint, CachedPose = elem }; @@ -219,7 +221,8 @@ protected virtual void LoadPosesAndAttachmentPoints(ShapeElement[] elements, Lis for (int j = 0; j < elem.AttachmentPoints.Length; j++) { AttachmentPoint apoint = elem.AttachmentPoints[j]; - AttachmentPointByCode[apoint.Code] = new AttachmentPointAndPose() { + AttachmentPointByCode[apoint.Code] = new AttachmentPointAndPose() + { AttachPoint = apoint, CachedPose = pose }; @@ -321,38 +324,54 @@ public override void OnFrame(Dictionary activeAnimati base.OnFrame(activeAnimationsByAnimCode, dt); } + // Override to calculate transformation matrices on each frame protected override void calculateMatrices(float dt) { - if (!CalculateMatrices) return; + // If matrix calculation is disabled, exit the method + if (!CalculateMatrices) + return; + // Clear the set of processed joints jointsDone.Clear(); + // Initialize animation version (used for caching transformations) int animVersion = 0; - for (int j = 0; j < activeAnimCount; j++) + // Limit the number of animations to process + int safeAnimCount = Math.Min(activeAnimCount, MaxConcurrentAnimations); + + // Process each active animation + for (int j = 0; j < safeAnimCount; j++) { RunningAnimation anim = CurAnims[j]; + // Set element weights for the current animation weightsByAnimationAndElement[0][j] = anim.ElementWeights; + // Track maximum animation version among active ones animVersion = Math.Max(animVersion, anim.Animation.Version); + // Get previous and next keyframes for current time AnimationFrame[] prevNextFrame = anim.Animation.PrevNextKeyFrameByFrame[(int)anim.CurrentFrame % anim.Animation.QuantityFrames]; frameByDepthByAnimation[0][j] = prevNextFrame[0].RootElementTransforms; prevFrame[j] = prevNextFrame[0].FrameNumber; + // Determine next frame based on animation end handling type if (anim.Animation.OnAnimationEnd == EnumEntityAnimationEndHandling.Hold && (int)anim.CurrentFrame + 1 == anim.Animation.QuantityFrames) { + // If animation should hold on the last frame nextFrameTransformsByAnimation[0][j] = prevNextFrame[0].RootElementTransforms; nextFrame[j] = prevNextFrame[0].FrameNumber; } else { + // Otherwise move to next frame for interpolation nextFrameTransformsByAnimation[0][j] = prevNextFrame[1].RootElementTransforms; nextFrame[j] = prevNextFrame[1].FrameNumber; } } + // Recursively calculate transformation matrices for all hierarchy elements calculateMatrices( animVersion, dt, @@ -364,144 +383,166 @@ protected override void calculateMatrices(float dt) 0 ); + // Initialize transformation matrices for joints not used in animations var TransformationMatrices = this.TransformationMatrices; if (TransformationMatrices != null) { - for (int jointidMul16 = 0; jointidMul16 < TransformationMatrices.Length; jointidMul16 += 16) // radfast 20.2.25: jointIdMul16 is (jointId * 16); I think this is the highest performance way to loop through this, taking account of the many array bounds checks + // Iterate through joint matrices (each matrix takes 16 elements) + for (int jointidMul16 = 0; jointidMul16 < TransformationMatrices.Length; jointidMul16 += 16) { - if (jointsById.ContainsKey(jointidMul16 / 16)) continue; + // Skip joints that are active in animation + if (jointsById.ContainsKey(jointidMul16 / 16)) + continue; - for (int j = 0; j < 16; j++) - { - TransformationMatrices[jointidMul16 + j] = identMat[j]; - } + // Copy identity matrix for unused joints + Buffer.BlockCopy(identMat, 0, TransformationMatrices, jointidMul16 * sizeof(float), 16 * sizeof(float)); } } + // Synchronize model matrices for attachment points foreach (var val in AttachmentPointByCode) { var cachedMatrix = val.Value.CachedPose.AnimModelMatrix; var animMatrix = val.Value.AnimModelMatrix; - for (int i = 0; i < 16; i++) - { - animMatrix[i] = cachedMatrix[i]; - } + // Copy current transformation matrix to attachment point animation matrix + Buffer.BlockCopy(cachedMatrix, 0, animMatrix, 0, 16 * sizeof(float)); } } - - - // Careful when changing around stuff in here, this is a recursively called method + // Recursive calculation of transformation matrices for hierarchy elements private void calculateMatrices( - int animVersion, - float dt, - List outFrame, - ShapeElementWeights[][] weightsByAnimationAndElement, - float[] modelMatrix, - List[] nowKeyFrameByAnimation, - List[] nextInKeyFrameByAnimation, - int depth - ) + int animVersion, + float dt, + List outFrame, + ShapeElementWeights[][] weightsByAnimationAndElement, + float[] modelMatrix, + List[] nowKeyFrameByAnimation, + List[] nextInKeyFrameByAnimation, + int depth + ) { + // Increment recursion depth depth++; List[] nowChildKeyFrameByAnimation = this.frameByDepthByAnimation[depth]; List[] nextChildKeyFrameByAnimation = this.nextFrameTransformsByAnimation[depth]; ShapeElementWeights[][] childWeightsByAnimationAndElement = this.weightsByAnimationAndElement[depth]; - localTransformMatrix ??= Mat4f.Create(); + + // Process each child element in current pose for (int childPoseIndex = 0; childPoseIndex < outFrame.Count; childPoseIndex++) { ElementPose outFramePose = outFrame[childPoseIndex]; ShapeElement elem = outFramePose.ForElement; - + // Set model matrix for current element outFramePose.SetMat(modelMatrix); + // Initialize local transformation matrix with identity matrix Mat4f.Identity(localTransformMatrix); - + // Clear pose before calculating weighted transformations outFramePose.Clear(); - float weightSum = 0f; + #if DEBUG - StringBuilder sb = null; - if (EleWeightDebug) sb = new StringBuilder(); + // Debug information about element weights + StringBuilder sb = null; + if (EleWeightDebug) sb = new StringBuilder(); #endif - for (int animIndex = 0; animIndex < activeAnimCount; animIndex++) + + // Limit the number of animations to process + int safeAnimCount = Math.Min(activeAnimCount, MaxConcurrentAnimations); + + // First pass: calculate total weight of all influencing animations + for (int animIndex = 0; animIndex < safeAnimCount; animIndex++) { - RunningAnimation anim = CurAnims[animIndex]; + if (childPoseIndex >= weightsByAnimationAndElement[animIndex].Length) + continue; + RunningAnimation anim = CurAnims[animIndex]; ShapeElementWeights sew = weightsByAnimationAndElement[animIndex][childPoseIndex]; - + // Only blending modes other than "Add" participate in weight normalization if (sew.BlendMode != EnumAnimationBlendMode.Add) { weightSum += sew.Weight * anim.EasingFactor; } - #if DEBUG - if (EleWeightDebug) sb.Append(string.Format("{0:0.0} from {1} (blendmode {2}), ", sew.Weight * anim.EasingFactor, anim.Animation.Code, sew.BlendMode)); + if (EleWeightDebug) sb.Append(string.Format("{0:0.0} from {1} (blendmode {2}), ", sew.Weight * anim.EasingFactor, anim.Animation.Code, sew.BlendMode)); #endif } #if DEBUG - if (EleWeightDebug) - { - if (eleWeights.ContainsKey(elem.Name)) eleWeights[elem.Name] += sb.ToString(); - else eleWeights[elem.Name] = sb.ToString(); - } + if (EleWeightDebug) + { + if (eleWeights.ContainsKey(elem.Name)) eleWeights[elem.Name] += sb.ToString(); + else eleWeights[elem.Name] = sb.ToString(); + } #endif - for (int animIndex = 0; animIndex < activeAnimCount; animIndex++) + // Second pass: blend transformations of all active animations + for (int animIndex = 0; animIndex < safeAnimCount; animIndex++) { + if (childPoseIndex >= weightsByAnimationAndElement[animIndex].Length) + continue; + RunningAnimation anim = CurAnims[animIndex]; ShapeElementWeights sew = weightsByAnimationAndElement[animIndex][childPoseIndex]; + // Calculate final weighted coefficient for current animation anim.CalcBlendedWeight(weightSum / sew.Weight, sew.BlendMode); ElementPose nowFramePose = nowKeyFrameByAnimation[animIndex][childPoseIndex]; ElementPose nextFramePose = nextInKeyFrameByAnimation[animIndex][childPoseIndex]; - int prevFrame = this.prevFrame[animIndex]; int nextFrame = this.nextFrame[animIndex]; - // May loop around, so nextFrame can be smaller than prevFrame + // Calculate distance between keyframes (accounting for cyclic animations) float keyFrameDist = nextFrame > prevFrame ? (nextFrame - prevFrame) : (anim.Animation.QuantityFrames - prevFrame + nextFrame); + // Calculate distance from previous frame to current animation frame float curFrameDist = anim.CurrentFrame >= prevFrame ? (anim.CurrentFrame - prevFrame) : (anim.Animation.QuantityFrames - prevFrame + anim.CurrentFrame); + // Calculate interpolation coefficient (0-1) between keyframes + float lerp = keyFrameDist < 1e-6f ? 0f : curFrameDist / keyFrameDist; - float lerp = curFrameDist / keyFrameDist; - + // Linear interpolation between current and next frame with animation weight applied outFramePose.Add(nowFramePose, nextFramePose, lerp, anim.BlendedWeight); + // Prepare data for recursive child element processing nowChildKeyFrameByAnimation[animIndex] = nowFramePose.ChildElementPoses; childWeightsByAnimationAndElement[animIndex] = sew.ChildElements; - nextChildKeyFrameByAnimation[animIndex] = nextFramePose.ChildElementPoses; } + // Get and apply local transformation matrix for element elem.GetLocalTransformMatrix(animVersion, localTransformMatrix, outFramePose); Mat4f.Mul(outFramePose.AnimModelMatrix, outFramePose.AnimModelMatrix, localTransformMatrix); - if (TransformationMatrices != null) // It is null on a server, non-null on a client + // If this is client (joint matrices are calculated), update joint transformation matrices + if (TransformationMatrices != null) { + // If element has a joint and this joint hasn't been processed yet if (elem.JointId > 0 && !jointsDone.Contains(elem.JointId)) { var tmpMatrixLocal = tmpMatrix ??= Mat4f.Create(); + // Apply inverse model transformation to get final joint matrix Mat4f.Mul(tmpMatrixLocal, outFramePose.AnimModelMatrix, elem.inverseModelTransform); - int index = 16 * elem.JointId; var transformationMatrices = TransformationMatrices; - if (index + 16 > transformationMatrices.Length) // Check we have space for this joint: we normally should, if MaxJointId was correct, but a mod could have modified the shape or something + + // Check if there's space in the matrix array (in case a mod extended the hierarchy) + if (index + 16 > transformationMatrices.Length) { var transformationMatricesDefault = this.TransformationMatricesDefaultPose; - initMatrices(elem.JointId + 1); // This replaces the matrices in both fields, but our local references are still the old populated matrices - Array.Copy(transformationMatrices, this.TransformationMatrices, transformationMatrices.Length); - Array.Copy(transformationMatricesDefault, this.TransformationMatricesDefaultPose, transformationMatricesDefault.Length); + // Re-initialize matrices with larger size + initMatrices(elem.JointId + 1); + // Copy previously calculated matrices to new array + Buffer.BlockCopy(transformationMatrices, 0, this.TransformationMatrices, 0, transformationMatrices.Length * sizeof(float)); + Buffer.BlockCopy(transformationMatricesDefault, 0, this.TransformationMatricesDefaultPose, 0, transformationMatricesDefault.Length * sizeof(float)); transformationMatrices = this.TransformationMatrices; } - for (int i = 0; i < 16; i++) - { - transformationMatrices[index + i] = tmpMatrixLocal[i]; - } + // Copy joint matrix to global transformation matrices array + Buffer.BlockCopy(tmpMatrix, 0, transformationMatrices, index * sizeof(float), 16 * sizeof(float)); + // Mark joint as processed jointsDone.Add(elem.JointId); } } + // Recursively process child elements if (outFramePose.ChildElementPoses != null) { calculateMatrices( diff --git a/Common/Model/Shape/ShapeElement.cs b/Common/Model/Shape/ShapeElement.cs index 685815e6..edf7676a 100644 --- a/Common/Model/Shape/ShapeElement.cs +++ b/Common/Model/Shape/ShapeElement.cs @@ -283,86 +283,267 @@ internal void TrimTextureNamesAndResolveFaces() } + /// + /// Cached base matrices for different animation versions + /// + private float[] _cachedBaseMatrixV0; + private float[] _cachedBaseMatrixV1; + + + /// + /// Flag indicating that the cache is stale + /// + private bool _isCacheDirty = true; + + /// + /// Static field for identity transformation + /// static ElementPose noTransform = new ElementPose(); + /// + /// Gets the local transformation matrix with caching + /// public float[] GetLocalTransformMatrix(int animVersion, float[] output = null, ElementPose tf = null) { if (tf == null) tf = noTransform; - - ShapeElement elem = this; output ??= Mat4f.Create(); - Span matrix = (Span)output; - float origin0 = 0f; - float origin1 = 0f; - float origin2 = 0f; - - if (elem.RotationOrigin != null) + // Check if cache needs to be updated + if (_isCacheDirty) { - origin0 = (float)elem.RotationOrigin[0] / 16; - origin1 = (float)elem.RotationOrigin[1] / 16; - origin2 = (float)elem.RotationOrigin[2] / 16; + RebuildCache(); } - // R = rotate, S = scale, T = translate - // Version 0: R * S * T - // Version 1: T * S * R + // Check if tf is empty transformation + bool isTfEmpty = IsTfEmpty(tf); if (animVersion == 1) { - // ================================================== - // BASE ELEMENT TRANSLATE SCALE ROTATE - // ================================================== - Mat4f.Translate(matrix, origin0, origin1, origin2); - - Mat4f.Scale(matrix, (float)elem.ScaleX, (float)elem.ScaleY, (float)elem.ScaleZ); + // For version 1 always use cached matrix + if (_cachedBaseMatrixV1 == null) + { + RebuildCache(); + } - Mat4f.RotateByXYZ(matrix, - (float)(elem.RotationX * GameMath.DEG2RAD), - (float)(elem.RotationY * GameMath.DEG2RAD), - (float)(elem.RotationZ * GameMath.DEG2RAD)); + // Copy cached matrix + Array.Copy(_cachedBaseMatrixV1, 0, output, 0, 16); - // note: lumped together with next translation - // Mat4f.Translate(output, output, -originX, -originY, -originZ); + // If tf is not empty, apply transformations + if (!isTfEmpty) + { + ApplyTfToMatrixV1(output, tf); + } + } + else + { + // For version 0 + if (isTfEmpty) + { + // If tf is empty, use cached matrix + if (_cachedBaseMatrixV0 == null) + { + RebuildCache(); + } + Array.Copy(_cachedBaseMatrixV0, 0, output, 0, 16); + } + else + { + // If tf is not empty, recalculate completely + ComputeMatrixV0WithTf(output, tf); + } + } - // ================================================== - // KEYFRAME TRANSLATE SCALE ROTATE - // ================================================== + return output; + } - Mat4f.Translate(matrix, - -origin0 + (float)elem.From[0] / 16 + tf.translateX, - -origin1 + (float)elem.From[1] / 16 + tf.translateY, - -origin2 + (float)elem.From[2] / 16 + tf.translateZ - ); + /// + /// Checks if transformation is empty + /// + private static bool IsTfEmpty(ElementPose tf) + { + return tf.translateX == 0 && tf.translateY == 0 && tf.translateZ == 0 && + tf.scaleX == 1 && tf.scaleY == 1 && tf.scaleZ == 1 && + tf.degX == 0 && tf.degY == 0 && tf.degZ == 0 && + tf.degOffX == 0 && tf.degOffY == 0 && tf.degOffZ == 0; + } - Mat4f.Scale(matrix, tf.scaleX, tf.scaleY, tf.scaleZ); - Mat4f.RotateByXYZ(matrix, - (float)(tf.degX + tf.degOffX) * GameMath.DEG2RAD, - (float)(tf.degY + tf.degOffY) * GameMath.DEG2RAD, - (float)(tf.degZ + tf.degOffZ) * GameMath.DEG2RAD); - } - else + /// + /// Rebuilds cached matrices + /// + private void RebuildCache() + { + float origin0 = 0f, origin1 = 0f, origin2 = 0f; + if (RotationOrigin != null) { - Mat4f.Translate(matrix, origin0, origin1, origin2); + origin0 = (float)RotationOrigin[0] / 16f; + origin1 = (float)RotationOrigin[1] / 16f; + origin2 = (float)RotationOrigin[2] / 16f; + } + + // Create matrices for caching + _cachedBaseMatrixV0 = Mat4f.Create(); + _cachedBaseMatrixV1 = Mat4f.Create(); + + // Calculate base matrix for version 0 + ComputeBaseMatrixV0(_cachedBaseMatrixV0, origin0, origin1, origin2); - Mat4f.RotateByXYZ(matrix, - (float)(elem.RotationX + tf.degX + tf.degOffX) * GameMath.DEG2RAD, - (float)(elem.RotationY + tf.degY + tf.degOffY) * GameMath.DEG2RAD, - (float)(elem.RotationZ + tf.degZ + tf.degOffZ) * GameMath.DEG2RAD); + // Calculate base matrix for version 1 + ComputeBaseMatrixV1(_cachedBaseMatrixV1, origin0, origin1, origin2); - Mat4f.Scale(matrix, (float)elem.ScaleX * tf.scaleX, (float)elem.ScaleY * tf.scaleY, (float)elem.ScaleZ * tf.scaleZ); + _isCacheDirty = false; + } + + /// + /// Calculates base matrix for version 0 + /// + private void ComputeBaseMatrixV0(float[] matrix, float origin0, float origin1, float origin2) + { + Mat4f.Identity(matrix); - Mat4f.Translate(matrix, - (float)elem.From[0] / 16 + tf.translateX - origin0, - (float)elem.From[1] / 16 + tf.translateY - origin1, - (float)elem.From[2] / 16 + tf.translateZ - origin2 - ); + if (origin0 != 0f || origin1 != 0f || origin2 != 0f) + Mat4f.Translate(matrix, matrix, origin0, origin1, origin2); + + float rotX = (float)RotationX * GameMath.DEG2RAD; + if (rotX != 0) + Mat4f.RotateX(matrix, matrix, rotX); + + float rotY = (float)RotationY * GameMath.DEG2RAD; + if (rotY != 0) + Mat4f.RotateY(matrix, matrix, rotY); + + float rotZ = (float)RotationZ * GameMath.DEG2RAD; + if (rotZ != 0) + Mat4f.RotateZ(matrix, matrix, rotZ); + + float scaleX = (float)ScaleX; + float scaleY = (float)ScaleY; + float scaleZ = (float)ScaleZ; + + if (scaleX != 1f || scaleY != 1f || scaleZ != 1f) + Mat4f.Scale(matrix, matrix, scaleX, scaleY, scaleZ); + + float tx = (float)From[0] / 16f - origin0; + float ty = (float)From[1] / 16f - origin1; + float tz = (float)From[2] / 16f - origin2; + + if (tx != 0f || ty != 0f || tz != 0f) + Mat4f.Translate(matrix, matrix, tx, ty, tz); + } + + /// + /// Calculates base matrix for version 1 + /// + private void ComputeBaseMatrixV1(float[] matrix, float origin0, float origin1, float origin2) + { + Mat4f.Identity(matrix); + + if (origin0 != 0f || origin1 != 0f || origin2 != 0f) + Mat4f.Translate(matrix, matrix, origin0, origin1, origin2); + + float scaleX = (float)ScaleX; + float scaleY = (float)ScaleY; + float scaleZ = (float)ScaleZ; + + if (scaleX != 1f || scaleY != 1f || scaleZ != 1f) + Mat4f.Scale(matrix, matrix, scaleX, scaleY, scaleZ); + + float rotX = (float)RotationX * GameMath.DEG2RAD; + if (rotX != 0) + Mat4f.RotateX(matrix, matrix, rotX); + + float rotY = (float)RotationY * GameMath.DEG2RAD; + if (rotY != 0) + Mat4f.RotateY(matrix, matrix, rotY); + + float rotZ = (float)RotationZ * GameMath.DEG2RAD; + if (rotZ != 0) + Mat4f.RotateZ(matrix, matrix, rotZ); + + float tx = -origin0 + (float)From[0] / 16f; + float ty = -origin1 + (float)From[1] / 16f; + float tz = -origin2 + (float)From[2] / 16f; + + if (tx != 0f || ty != 0f || tz != 0f) + Mat4f.Translate(matrix, matrix, tx, ty, tz); + } + + /// + /// Applies transformation tf to version 1 matrix + /// + private void ApplyTfToMatrixV1(float[] matrix, ElementPose tf) + { + // Apply translation + if (tf.translateX != 0f || tf.translateY != 0f || tf.translateZ != 0f) + Mat4f.Translate(matrix, matrix, tf.translateX, tf.translateY, tf.translateZ); + + // Apply scaling + if (tf.scaleX != 1f || tf.scaleY != 1f || tf.scaleZ != 1f) + Mat4f.Scale(matrix, matrix, tf.scaleX, tf.scaleY, tf.scaleZ); + + // Apply rotation + float rotX = (float)(tf.degX + tf.degOffX) * GameMath.DEG2RAD; + if (rotX != 0) + Mat4f.RotateX(matrix, matrix, rotX); + + float rotY = (float)(tf.degY + tf.degOffY) * GameMath.DEG2RAD; + if (rotY != 0) + Mat4f.RotateY(matrix, matrix, rotY); + + float rotZ = (float)(tf.degZ + tf.degOffZ) * GameMath.DEG2RAD; + if (rotZ != 0) + Mat4f.RotateZ(matrix, matrix, rotZ); + } + + /// + /// Calculates version 0 matrix with tf transformations + /// + private void ComputeMatrixV0WithTf(float[] output, ElementPose tf) + { + float origin0 = 0f, origin1 = 0f, origin2 = 0f; + if (RotationOrigin != null) + { + origin0 = (float)RotationOrigin[0] / 16f; + origin1 = (float)RotationOrigin[1] / 16f; + origin2 = (float)RotationOrigin[2] / 16f; } - return output; + Mat4f.Identity(output); + + if (origin0 != 0f || origin1 != 0f || origin2 != 0f) + Mat4f.Translate(output, output, origin0, origin1, origin2); + + float rotX = (float)(RotationX + tf.degX + tf.degOffX) * GameMath.DEG2RAD; + if (rotX != 0) + Mat4f.RotateX(output, output, rotX); + + float rotY = (float)(RotationY + tf.degY + tf.degOffY) * GameMath.DEG2RAD; + if (rotY != 0) + Mat4f.RotateY(output, output, rotY); + + float rotZ = (float)(RotationZ + tf.degZ + tf.degOffZ) * GameMath.DEG2RAD; + if (rotZ != 0) + Mat4f.RotateZ(output, output, rotZ); + + float scaleX = (float)ScaleX * tf.scaleX; + float scaleY = (float)ScaleY * tf.scaleY; + float scaleZ = (float)ScaleZ * tf.scaleZ; + + if (scaleX != 1f || scaleY != 1f || scaleZ != 1f) + Mat4f.Scale(output, output, scaleX, scaleY, scaleZ); + + float tx = (float)From[0] / 16f + tf.translateX - origin0; + float ty = (float)From[1] / 16f + tf.translateY - origin1; + float tz = (float)From[2] / 16f + tf.translateZ - origin2; + + if (tx != 0f || ty != 0f || tz != 0f) + Mat4f.Translate(output, output, tx, ty, tz); } + + /// + /// Clones the element while preserving cache + /// public ShapeElement Clone() { ShapeElement elem = new ShapeElement() @@ -388,7 +569,13 @@ public ShapeElement Clone() ScaleX = ScaleX, ScaleY = ScaleY, ScaleZ = ScaleZ, - Name = Name + Name = Name, + + // Copy cache + _cachedBaseMatrixV0 = (float[])_cachedBaseMatrixV0?.Clone(), + _cachedBaseMatrixV1 = (float[])_cachedBaseMatrixV1?.Clone(), + + _isCacheDirty = _isCacheDirty }; var Children = this.Children; @@ -404,7 +591,6 @@ public ShapeElement Clone() } return elem; - } public void SetJointIdRecursive(int jointId) diff --git a/Math/Matrix/Mat4f.cs b/Math/Matrix/Mat4f.cs index 23c54926..f12b69f0 100644 --- a/Math/Matrix/Mat4f.cs +++ b/Math/Matrix/Mat4f.cs @@ -1,26 +1,26 @@ -//glMatrix license: -//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. - -//Redistribution and use in source and binary forms, with or without modification, -//are permitted provided that the following conditions are met: - -// * Redistributions of source code must retain the above copyright notice, this -// list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. - -//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON -//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - +//glMatrix license: +//Copyright (c) 2013, Brandon Jones, Colin MacKenzie IV. All rights reserved. + +//Redistribution and use in source and binary forms, with or without modification, +//are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +//ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +//ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + using Cairo; using System; @@ -31,15 +31,15 @@ namespace Vintagestory.API.MathTools { /// /// 4x4 Matrix Math - /// + /// public class Mat4f { /// - /// Creates a new identity mat4 - /// 0 4 8 12 - /// 1 5 9 13 - /// 2 6 10 14 - /// 3 7 11 15 + /// Creates a new identity mat4 + /// 0 4 8 12 + /// 1 5 9 13 + /// 2 6 10 14 + /// 3 7 11 15 /// /// {mat4} a new 4x4 matrix public static float[] Create() @@ -65,10 +65,10 @@ public static void NewIdentity(Span output) } /// - /// Creates a new mat4 initialized with values from an existing matrix + /// Creates a new mat4 initialized with values from an existing matrix /// /// a matrix to clone - /// {mat4} a new 4x4 matrix + /// {mat4} a new 4x4 matrix public static float[] CloneIt(float[] a) { float[] output = new float[16]; @@ -96,7 +96,7 @@ public static float[] CloneIt(float[] a) /// /// {mat4} out the receiving matrix /// {mat4} a the source matrix - /// {mat4} out + /// {mat4} out public static float[] Copy(float[] output, Span a) { output[0] = a[0]; @@ -122,7 +122,7 @@ public static float[] Copy(float[] output, Span a) /// Set a mat4 to the identity matrix /// /// {mat4} out the receiving matrix - /// {mat4} out + /// {mat4} out public static float[] Identity(float[] output) { output[0] = 1; @@ -150,7 +150,7 @@ public static float[] Identity(float[] output) /// /// {mat4} out the receiving matrix /// - /// {mat4} out + /// {mat4} out public static float[] Identity_Scaled(float[] output, float scale) { output[0] = scale; @@ -178,7 +178,7 @@ public static float[] Identity_Scaled(float[] output, float scale) /// /// {mat4} out the receiving matrix /// {mat4} a the source matrix - /// {mat4} out + /// {mat4} out public static float[] Transpose(float[] output, float[] a) { // If we are transposing ourselves we can skip a few steps but have to cache some values @@ -225,11 +225,11 @@ public static float[] Transpose(float[] output, float[] a) } /// - /// Inverts a mat4 + /// Inverts a mat4 /// /// {mat4} out the receiving matrix /// {mat4} a the source matrix - /// {mat4} out + /// {mat4} out public static float[] Invert(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -331,7 +331,7 @@ public static void Invert(Span a) /// /// {mat4} out the receiving matrix /// {mat4} a the source matrix - /// {mat4} out + /// {mat4} out public static float[] Adjoint(float[] output, float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -362,7 +362,7 @@ public static float[] Adjoint(float[] output, float[] a) /// Calculates the determinant of a mat4 /// /// {mat4} a the source matrix - /// {Number} determinant of a + /// {Number} determinant of a public static float Determinant(float[] a) { float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; @@ -390,45 +390,57 @@ public static float Determinant(float[] a) } /// - /// Multiplies two mat4's + /// Multiplies two mat4's /// /// /// {mat4} out the receiving matrix /// {mat4} a the first operand /// {mat4} b the second operand - /// {mat4} out - public static float[] Multiply(float[] output, float[] a, float[] b) + /// {mat4} out + public static unsafe float[] Multiply(float[] output, float[] a, float[] b) { - float a00 = a[0]; float a01 = a[1]; float a02 = a[2]; float a03 = a[3]; - float a10 = a[4]; float a11 = a[5]; float a12 = a[6]; float a13 = a[7]; - float a20 = a[8]; float a21 = a[9]; float a22 = a[10]; float a23 = a[11]; - float a30 = a[12]; float a31 = a[13]; float a32 = a[14]; float a33 = a[15]; - - // Cache only the current line of the second matrix - float b0 = b[0]; float b1 = b[1]; float b2 = b[2]; float b3 = b[3]; - output[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - output[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - output[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - output[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[4]; b1 = b[5]; b2 = b[6]; b3 = b[7]; - output[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - output[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - output[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - output[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[8]; b1 = b[9]; b2 = b[10]; b3 = b[11]; - output[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - output[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - output[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - output[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[12]; b1 = b[13]; b2 = b[14]; b3 = b[15]; - output[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - output[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - output[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - output[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - return output; + // Use unsafe for direct memory access + fixed (float* pa = a, pb = b, pout = output) + { + // Cache rows of matrix A + float a00 = pa[0], a01 = pa[1], a02 = pa[2], a03 = pa[3]; + float a10 = pa[4], a11 = pa[5], a12 = pa[6], a13 = pa[7]; + float a20 = pa[8], a21 = pa[9], a22 = pa[10], a23 = pa[11]; + float a30 = pa[12], a31 = pa[13], a32 = pa[14], a33 = pa[15]; + + // Optimized multiplication using pointers + float* bPtr = pb; + float* outPtr = pout; + + // Row 0 + float b0 = bPtr[0], b1 = bPtr[1], b2 = bPtr[2], b3 = bPtr[3]; + outPtr[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + outPtr[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + outPtr[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + outPtr[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + // Row 1 + b0 = bPtr[4]; b1 = bPtr[5]; b2 = bPtr[6]; b3 = bPtr[7]; + outPtr[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + outPtr[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + outPtr[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + outPtr[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + // Row 2 + b0 = bPtr[8]; b1 = bPtr[9]; b2 = bPtr[10]; b3 = bPtr[11]; + outPtr[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + outPtr[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + outPtr[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + outPtr[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + + // Row 3 + b0 = bPtr[12]; b1 = bPtr[13]; b2 = bPtr[14]; b3 = bPtr[15]; + outPtr[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; + outPtr[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; + outPtr[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; + outPtr[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; + } + return output; } public static void Mul(Span a, Span b) @@ -470,7 +482,7 @@ public static void Mul(Span a, Span b) /// /// /// - /// + /// public static float[] Mul(float[] output, float[] a, float[] b) { return Multiply(output, a, b); @@ -485,7 +497,7 @@ public static float[] Mul(float[] output, float[] a, float[] b) /// {vec3} v vector to translate by /// /// - /// {mat4} out + /// {mat4} out public static float[] Translate(float[] output, float[] input, float x, float y, float z) { if (input == output) @@ -540,7 +552,7 @@ public static void Translate(Span matrix, float x, float y, float z) /// {mat4} out the receiving matrix /// {mat4} a the matrix to translate /// {vec3} v vector to translate by - /// {mat4} out + /// {mat4} out public static float[] Translate(float[] output, float[] input, float[] translate) { float x = translate[0]; float y = translate[1]; float z = translate[2]; @@ -580,7 +592,7 @@ public static float[] Translate(float[] output, float[] input, float[] translate /// {mat4} out the receiving matrix /// {mat4} a the matrix to scale /// {vec3} v the vec3 to scale the matrix by - /// {mat4} out + /// {mat4} out public static float[] Scale(float[] output, float[] a, float[] v) { float x = v[0]; float y = v[1]; float z = v[2]; @@ -648,7 +660,7 @@ public static void SimpleScaleMatrix(Span matrix, float x, float y, float /// /// /// - /// {mat4} out + /// {mat4} out public static float[] Scale(float[] output, float[] a, float xScale, float yScale, float zScale) { output[0] = a[0] * xScale; @@ -680,7 +692,7 @@ public static float[] Scale(float[] output, float[] a, float xScale, float yScal /// {mat4} a the matrix to rotate /// {Number} rad the angle to rotate the matrix by /// {vec3} axis the axis to rotate around - /// {mat4} out + /// {mat4} out public static float[] Rotate(float[] output, float[] a, float rad, float[] axis) { float x = axis[0]; float y = axis[1]; float z = axis[2]; @@ -744,7 +756,7 @@ public static float[] Rotate(float[] output, float[] a, float rad, float[] axis) /// {mat4} out the receiving matrix /// {mat4} a the matrix to rotate /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// {mat4} out public static float[] RotateX(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -789,7 +801,7 @@ public static float[] RotateX(float[] output, float[] a, float rad) /// {mat4} out the receiving matrix /// {mat4} a the matrix to rotate /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// {mat4} out public static float[] RotateY(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -834,7 +846,7 @@ public static float[] RotateY(float[] output, float[] a, float rad) /// {mat4} out the receiving matrix /// {mat4} a the matrix to rotate /// {Number} rad the angle to rotate the matrix by - /// {mat4} out + /// {mat4} out public static float[] RotateZ(float[] output, float[] a, float rad) { float s = GameMath.Sin(rad); @@ -996,18 +1008,18 @@ public static void RotateByXYZ(Span matrix, float radX, float radY, float } /// - /// Creates a matrix from a quaternion rotation and vector translation - /// This is equivalent to (but much faster than): - /// mat4.identity(dest); - /// mat4.translate(dest, vec); - /// var quatMat = mat4.create(); - /// quat4.toMat4(quat, quatMat); + /// Creates a matrix from a quaternion rotation and vector translation + /// This is equivalent to (but much faster than): + /// mat4.identity(dest); + /// mat4.translate(dest, vec); + /// var quatMat = mat4.create(); + /// quat4.toMat4(quat, quatMat); /// mat4.multiply(dest, quatMat); /// /// {mat4} out mat4 receiving operation result /// {quat4} q Rotation quaternion /// {vec3} v Translation vector - /// {mat4} out + /// {mat4} out public static float[] FromRotationTranslation(float[] output, float[] q, float[] v) { // Quaternion math @@ -1051,7 +1063,7 @@ public static float[] FromRotationTranslation(float[] output, float[] q, float[] /// /// {mat4} out mat4 receiving operation result /// {quat} q Quaternion to create matrix from - /// {mat4} out + /// {mat4} out public static float[] FromQuat(float[] output, float[] q) { float x = q[0]; float y = q[1]; float z = q[2]; float w = q[3]; @@ -1159,7 +1171,7 @@ public static void MulQuat(Span a, double[] q) /// {Number} top Top bound of the frustum /// {Number} near Near bound of the frustum /// {Number} far Far bound of the frustum - /// {mat4} out + /// {mat4} out public static float[] Frustum(float[] output, float left, float right, float bottom, float top, float near, float far) { float rl = 1 / (right - left); @@ -1192,7 +1204,7 @@ public static float[] Frustum(float[] output, float left, float right, float bot /// {number} aspect Aspect ratio. typically viewport width/height /// {number} near Near bound of the frustum /// {number} far Far bound of the frustum - /// {mat4} out + /// {mat4} out public static float[] Perspective(float[] output, float fovy, float aspect, float near, float far) { float one = 1; @@ -1227,7 +1239,7 @@ public static float[] Perspective(float[] output, float fovy, float aspect, floa /// {number} top Top bound of the frustum /// {number} near Near bound of the frustum /// {number} far Far bound of the frustum - /// {mat4} out + /// {mat4} out public static float[] Ortho(float[] output, float left, float right, float bottom, float top, float near, float far) { float lr = 1 / (left - right); @@ -1259,7 +1271,7 @@ public static float[] Ortho(float[] output, float left, float right, float botto /// {vec3} eye Position of the viewer /// {vec3} center Point the viewer is looking at /// {vec3} up vec3 pointing up - /// {mat4} out + /// {mat4} out public static float[] LookAt(float[] output, float[] eye, float[] center, float[] up) { float x0; float x1; float x2; float y0; float y1; float y2; float z0; float z1; float z2; float len; @@ -1352,7 +1364,7 @@ public static float[] LookAt(float[] output, float[] eye, float[] center, float[ /// /// /// - /// + /// public static float[] MulWithVec4(float[] matrix, float[] vec4) { float[] output = new float[] { 0, 0, 0, 0 }; @@ -1593,7 +1605,7 @@ public static double[] MulWithVec4(float[] matrix, double[] vec4) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, float[] vec4, Vec4f outVal) { outVal.Set(0, 0, 0, 0); @@ -1614,7 +1626,7 @@ public static void MulWithVec4(float[] matrix, float[] vec4, Vec4f outVal) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, Vec4d inVal, Vec4d outVal) { outVal.Set(0, 0, 0, 0); @@ -1636,7 +1648,7 @@ public static void MulWithVec4(float[] matrix, Vec4d inVal, Vec4d outVal) /// /// /// - /// + /// public static void MulWithVec4(float[] matrix, Vec4f inVal, Vec4f outVal) { outVal.Set(0, 0, 0, 0); @@ -1698,3 +1710,4 @@ public static float GLMAT_EPSILON() } } } +