protected override void ConfigureCollidable(TriangleEntry entry, float dt) { var shape = entry.Collidable.Shape; mesh.Shape.TriangleMesh.Data.GetTriangle(entry.Index, out shape.vA, out shape.vB, out shape.vC); Matrix3f o; Matrix3f.FromQuaternion(ref mesh.worldTransform.Orientation, out o); Vector3f.Transform(ref shape.vA, ref o, out shape.vA); Vector3f.Transform(ref shape.vB, ref o, out shape.vB); Vector3f.Transform(ref shape.vC, ref o, out shape.vC); Vector3f center; Vector3f.Add(ref shape.vA, ref shape.vB, out center); Vector3f.Add(ref center, ref shape.vC, out center); Vector3f.Multiply(ref center, 1 / 3f, out center); Vector3f.Subtract(ref shape.vA, ref center, out shape.vA); Vector3f.Subtract(ref shape.vB, ref center, out shape.vB); Vector3f.Subtract(ref shape.vC, ref center, out shape.vC); Vector3f.Add(ref center, ref mesh.worldTransform.Position, out center); //The bounding box doesn't update by itself. entry.Collidable.worldTransform.Position = center; entry.Collidable.worldTransform.Orientation = Quaternion.Identity; entry.Collidable.UpdateBoundingBoxInternal(dt); }
/// <summary> /// Removes a child from a compound body. /// </summary> /// <param name="childContributions">List of distribution information associated with each child shape of the whole compound shape used by the compound being split.</param> /// <param name="removalPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param> /// <param name="distributionInfo">Volume, volume distribution, and center information about the new form of the original compound collidable.</param> /// <param name="compound">Original compound to have a child removed.</param> /// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns> public static bool RemoveChildFromCompound(Entity <CompoundCollidable> compound, Func <CompoundChild, bool> removalPredicate, IList <ShapeDistributionInformation> childContributions, out ShapeDistributionInformation distributionInfo) { float weight; float removedWeight; Vector3f removedCenter; if (RemoveChildFromCompound(compound.CollisionInformation, removalPredicate, childContributions, out distributionInfo, out weight, out removedWeight, out removedCenter)) { //Reconfigure the entities using the data computed in the split. //Only bother if there are any children left in the compound! if (compound.CollisionInformation.Children.Count > 0) { float originalMass = compound.mass; float newMass = (weight / (weight + removedWeight)) * originalMass; Matrix3f.Multiply(ref distributionInfo.VolumeDistribution, newMass * InertiaHelper.InertiaTensorScale, out distributionInfo.VolumeDistribution); compound.Initialize(compound.CollisionInformation, newMass, distributionInfo.VolumeDistribution); RemoveReposition(compound, ref distributionInfo, weight, removedWeight, ref removedCenter); } return(true); } else { return(false); } }
//TODO: Include warnings about multithreading. These modify things outside of the entity and use single-thread-only helpers. ///<summary> /// Forces the entity to become kinematic. Kinematic entities have infinite mass and inertia. ///</summary> public void BecomeKinematic() { bool previousState = isDynamic; isDynamic = false; LocalInertiaTensorInverse = new Matrix3f(); mass = 0; inverseMass = 0; //Notify simulation island of the change. if (previousState) { if (activityInformation.DeactivationManager != null) { activityInformation.DeactivationManager.RemoveSimulationIslandFromMember(activityInformation); } if (((IForceUpdateable)this).ForceUpdater != null) { ((IForceUpdateable)this).ForceUpdater.ForceUpdateableBecomingKinematic(this); } } //Change the collision group if it was using the default. if (collisionInformation.CollisionRules.Group == CollisionRules.DefaultDynamicCollisionGroup || collisionInformation.CollisionRules.Group == null) { collisionInformation.CollisionRules.Group = CollisionRules.DefaultKinematicCollisionGroup; } activityInformation.Activate(); //Preserve velocity and reinitialize momentum for new state. LinearVelocity = linearVelocity; AngularVelocity = angularVelocity; }
public static int Main() { Matrix3f M3 = new Matrix3f(); M3.Set(0, 0, 1); M3[2, 2] = 1; Example.Print(M3); var D2 = new float[2, 2] { { 0.0f, 1.0f }, { 2.0f, 3.0f } }; var M2 = new Matrix2f(D2); Example.Print(M2); var D23 = new float[2, 3] { { 0.0f, 1.0f, 2.0f }, { 3.0f, 4.0f, 5.0f } }; var M23 = new MatrixXf(D23); Example.Print(M23); var S = new float[2, 3]; M23.CopyTo(S); Print(S); var T = new float[1, 1] { { 0.0f } }; M23.CopyTo(ref T); Print(T); return(0); }
///<summary> /// Constructs a new transformable shape. ///</summary> ///<param name="shape">Base shape to transform.</param> ///<param name="transform">Transform to use.</param> public TransformableShape(ConvexShape shape, Matrix3f transform) { this.shape = shape; this.transform = transform; UpdateConvexShapeInfo(); }
/// <summary> /// Multiplies the two matrices. /// </summary> /// <param name="left">First matrix to multiply.</param> /// <param name="right">Second matrix to multiply.</param> /// <returns>Product of the multiplication.</returns> public static Matrix3x2f Multiply(Matrix3f left, Matrix3x2f right) { Matrix3x2f res; Multiply(ref left, ref right, out res); return(res); }
///<summary> /// Constructs a new transformable shape. ///</summary> /// <param name="shape">Base shape to transform.</param> /// <param name="transform">Transform to use.</param> /// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param> public TransformableShape(ConvexShape shape, Matrix3f transform, ConvexShapeDescription description) { this.shape = shape; this.transform = transform; UpdateConvexShapeInfo(description); }
/// <summary> /// Creates a quaternion from a rotation matrix. /// </summary> /// <param name="rotation">Rotation matrix used to create a new quaternion.</param> /// <returns>Quaternion representing the same rotation as the matrix.</returns> public static Quaternion FromRotationMatrix(Matrix3f rotation) { Quaternion toReturn; FromRotationMatrix(ref rotation, out toReturn); return(toReturn); }
/// <summary> /// Transform this quaternion to equivalent matrix. /// </summary> /// <returns></returns> public Matrix3f ToRotationMatrix() { float ww = W * W; float xx = X * X; float yy = Y * Y; float zz = Z * Z; float wx = W * X; float wy = W * Y; float wz = W * Z; float xy = X * Y; float xz = X * Z; float yz = Y * Z; Matrix3f result = Matrix3f.Zero; result.Column0 = new Vector3f( 2 * (xx + ww) - 1, 2 * (xy + wz), 2 * (xz - wy)); result.Column1 = new Vector3f( 2 * (xy - wz), 2 * (yy + ww) - 1, 2 * (yz + wx)); result.Column2 = new Vector3f( 2 * (xz + wy), 2 * (yz - wx), 2 * (zz + ww) - 1); return(result); }
void IPositionUpdateable.PreUpdatePosition(float dt) { Vector3f increment; Vector3f.Multiply(ref angularVelocity, dt * .5f, out increment); var multiplier = new Quaternion(0, increment.X, increment.Y, increment.Z); Quaternion.Multiply(ref multiplier, ref orientation, out multiplier); Quaternion.Add(ref orientation, ref multiplier, out orientation); orientation.Normalize(); Matrix3f.FromQuaternion(ref orientation, out orientationMatrix); //Only do the linear motion if this object doesn't obey CCD. if (PositionUpdateMode == PositionUpdateMode.Discrete) { Vector3f.Multiply(ref linearVelocity, dt, out increment); Vector3f.Add(ref position, ref increment, out position); collisionInformation.UpdateWorldTransform(ref position, ref orientation); //The position update is complete if this is a discretely updated object. if (PositionUpdated != null) { PositionUpdated(this); } } MathChecker.Validate(linearVelocity); MathChecker.Validate(angularVelocity); MathChecker.Validate(position); MathChecker.Validate(orientation); #if CONSERVE MathChecker.Validate(angularMomentum); #endif }
/// <summary> /// Computes the volume distribution and center of the shape. /// </summary> /// <param name="entries">Mass-weighted entries of the compound.</param> /// <param name="volume">Summed volume of the constituent shapes. Intersecting volumes get double counted.</param> /// <param name="volumeDistribution">Volume distribution of the shape.</param> /// <param name="center">Center of the compound.</param> public static void ComputeVolumeDistribution(IList <CompoundShapeEntry> entries, out float volume, out Matrix3f volumeDistribution, out Vector3f center) { center = new Vector3f(); float totalWeight = 0; volume = 0; for (int i = 0; i < entries.Count; i++) { center += entries[i].LocalTransform.Position * entries[i].Weight; volume += entries[i].Shape.Volume; totalWeight += entries[i].Weight; } if (totalWeight <= 0) { throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive."); } float totalWeightInverse = 1 / totalWeight; totalWeightInverse.Validate(); center *= totalWeightInverse; volumeDistribution = new Matrix3f(); for (int i = 0; i < entries.Count; i++) { RigidTransform transform = entries[i].LocalTransform; Matrix3f contribution; TransformContribution(ref transform, ref center, ref entries[i].Shape.volumeDistribution, entries[i].Weight, out contribution); Matrix3f.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } Matrix3f.Multiply(ref volumeDistribution, totalWeightInverse, out volumeDistribution); volumeDistribution.Validate(); }
/// <summary> /// Gets the bounding box of the shape given a transform. /// </summary> /// <param name="shapeTransform">Transform to use.</param> /// <param name="boundingBox">Bounding box of the transformed shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3f o; Matrix3f.FromQuaternion(ref shapeTransform.Orientation, out o); //Sample the local directions from the orientation matrix, implicitly transposed. //Notice only three directions are used. Due to box symmetry, 'left' is just -right. var right = new Vector3f(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength); var up = new Vector3f(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength); var backward = new Vector3f(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength); //Rather than transforming each axis independently (and doing three times as many operations as required), just get the 3 required values directly. Vector3f offset; TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out offset); //The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes. Vector3f.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max); Vector3f.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min); }
/// <summary> /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// </summary> /// <param name="splitPredicate">Delegate which determines if a child in the original compound should be moved to the new compound.</param> /// <param name="distributionInfoA">Volume, volume distribution, and center information about the new form of the original compound collidable.</param> /// <param name="distributionInfoB">Volume, volume distribution, and center information about the new compound collidable.</param> /// <param name="a">Original compound to be split. Children in this compound will be removed and added to the other compound.</param> /// <param name="b">Compound to receive children removed from the original compound.</param> /// <returns>Whether or not the predicate returned true for any element in the original compound and split the compound.</returns> public static bool SplitCompound(Func <CompoundChild, bool> splitPredicate, Entity <CompoundCollidable> a, Entity <CompoundCollidable> b, out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB) { float weightA, weightB; if (SplitCompound(splitPredicate, a.CollisionInformation, b.CollisionInformation, out distributionInfoA, out distributionInfoB, out weightA, out weightB)) { //Reconfigure the entities using the data computed in the split. float originalMass = a.mass; if (a.CollisionInformation.children.Count > 0) { float newMassA = (weightA / (weightA + weightB)) * originalMass; Matrix3f.Multiply(ref distributionInfoA.VolumeDistribution, newMassA * InertiaHelper.InertiaTensorScale, out distributionInfoA.VolumeDistribution); a.Initialize(a.CollisionInformation, newMassA, distributionInfoA.VolumeDistribution); } if (b.CollisionInformation.children.Count > 0) { float newMassB = (weightB / (weightA + weightB)) * originalMass; Matrix3f.Multiply(ref distributionInfoB.VolumeDistribution, newMassB * InertiaHelper.InertiaTensorScale, out distributionInfoB.VolumeDistribution); b.Initialize(b.CollisionInformation, newMassB, distributionInfoB.VolumeDistribution); } SplitReposition(a, b, ref distributionInfoA, ref distributionInfoB, weightA, weightB); return(true); } return(false); }
/// <summary> Sets the value of this axis-angle to the rotational component of /// the passed matrix. /// If the specified matrix has no rotational component, the value /// of this AxisAngle4f is set to an angle of 0 about an axis of (0,1,0). /// </summary> /// <param name="m1">the matrix3f /// </param> public void set_Renamed(Matrix3f m1) { x = (float)(m1.m21 - m1.m12); y = (float)(m1.m02 - m1.m20); z = (float)(m1.m10 - m1.m01); double mag = x * x + y * y + z * z; if (mag > EPS) { mag = System.Math.Sqrt(mag); double sin = 0.5 * mag; double cos = 0.5 * (m1.m00 + m1.m11 + m1.m22 - 1.0); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" angle = (float)System.Math.Atan2(sin, cos); double invMag = 1.0 / mag; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" x = (float)(x * invMag); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" y = (float)(y * invMag); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" z = (float)(z * invMag); } else { x = 0.0f; y = 1.0f; z = 0.0f; angle = 0.0f; } }
protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobian = new Matrix3f(); Vector3f boneAxis; Vector3f.Transform(ref BoneLocalFreeAxis, ref TargetBone.Orientation, out boneAxis); angularJacobian = new Matrix3f { M11 = constrainedAxis1.X, M12 = constrainedAxis1.Y, M13 = constrainedAxis1.Z, M21 = constrainedAxis2.X, M22 = constrainedAxis2.Y, M23 = constrainedAxis2.Z }; Vector3f error; Vector3f.Cross(ref boneAxis, ref freeAxis, out error); Vector2f constraintSpaceError; Vector3f.Dot(ref error, ref constrainedAxis1, out constraintSpaceError.X); Vector3f.Dot(ref error, ref constrainedAxis2, out constraintSpaceError.Y); velocityBias.X = errorCorrectionFactor * constraintSpaceError.X; velocityBias.Y = errorCorrectionFactor * constraintSpaceError.Y; }
/// <summary> /// Modifies a contribution using a transform, position, and weight. /// </summary> /// <param name="transform">Transform to use to modify the contribution.</param> /// <param name="center">Center to use to modify the contribution.</param> /// <param name="baseContribution">Original unmodified contribution.</param> /// <param name="weight">Weight of the contribution.</param> /// <param name="contribution">Transformed contribution.</param> public static void TransformContribution(ref RigidTransform transform, ref Vector3f center, ref Matrix3f baseContribution, float weight, out Matrix3f contribution) { Matrix3f rotation; Matrix3f.FromQuaternion(ref transform.Orientation, out rotation); Matrix3f temp; //Do angular transformed contribution first... Matrix3f.MultiplyTransposed(ref rotation, ref baseContribution, out temp); Matrix3f.Multiply(ref temp, ref rotation, out temp); contribution = temp; //Now add in the offset from the origin. Vector3f offset; Vector3f.Subtract(ref transform.Position, ref center, out offset); Matrix3f innerProduct; Matrix3f.CreateScale(offset.LengthSquared, out innerProduct); Matrix3f outerProduct; Matrix3f.CreateOuterProduct(ref offset, ref offset, out outerProduct); Matrix3f.Subtract(ref innerProduct, ref outerProduct, out temp); Matrix3f.Add(ref contribution, ref temp, out contribution); Matrix3f.Multiply(ref contribution, weight, out contribution); }
protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobianA = Matrix3f.Identity; //The jacobian entries are is [ La, Aa, -Lb, -Ab ] because the relative velocity is computed using A-B. So, negate B's jacobians! linearJacobianB = new Matrix3f { M11 = -1, M22 = -1, M33 = -1 }; Vector3f rA; Vector3f.Transform(ref LocalOffsetA, ref ConnectionA.Orientation, out rA); Matrix3f.CreateCrossProduct(ref rA, out angularJacobianA); //Transposing a skew-symmetric matrix is equivalent to negating it. Matrix3f.Transpose(ref angularJacobianA, out angularJacobianA); Vector3f worldPositionA; Vector3f.Add(ref ConnectionA.Position, ref rA, out worldPositionA); Vector3f rB; Vector3f.Transform(ref LocalOffsetB, ref ConnectionB.Orientation, out rB); Matrix3f.CreateCrossProduct(ref rB, out angularJacobianB); Vector3f worldPositionB; Vector3f.Add(ref ConnectionB.Position, ref rB, out worldPositionB); Vector3f linearError; Vector3f.Subtract(ref worldPositionB, ref worldPositionA, out linearError); Vector3f.Multiply(ref linearError, errorCorrectionFactor, out velocityBias); }
/// <summary> /// Constructs a quaternion from a rotation matrix. /// </summary> /// <param name="rotation">Rotation matrix to create the quaternion from.</param> /// <param name="quaternion">Quaternion based on the rotation matrix.</param> public static void FromRotationMatrix(ref Matrix4f rotation, out Quaternion quaternion) { Matrix3f downsizedMatrix; Matrix3f.FromMatrix4f(ref rotation, out downsizedMatrix); FromRotationMatrix(ref downsizedMatrix, out quaternion); }
///<summary> /// Transforms a vector by an affine transform's inverse. ///</summary> ///<param name="position">Position to transform.</param> ///<param name="transform">Transform to invert and apply.</param> ///<param name="transformed">Transformed position.</param> public static void TransformInverse(ref Vector3f position, ref AffineTransform transform, out Vector3f transformed) { Vector3f.Subtract(ref position, ref transform.Translation, out transformed); Matrix3f inverse; Matrix3f.Invert(ref transform.LinearTransform, out inverse); Vector3f.TransformTranspose(ref transformed, ref inverse, out transformed); }
/// <summary> /// Creates an affine transform from a rigid transform. /// </summary> /// <param name="rigid">Rigid transform to base the affine transform on.</param> /// <returns>Affine transform created from the rigid transform.</returns> public static AffineTransform CreateFromRigidTransform(RigidTransform rigid) { AffineTransform toReturn; toReturn.Translation = rigid.Position; Matrix3f.FromQuaternion(ref rigid.Orientation, out toReturn.LinearTransform); return(toReturn); }
/// <summary> Sets the value of this quaternion to the rotational component of /// the passed matrix. /// </summary> /// <param name="m1">the matrix3f /// </param> public void set_Renamed(Matrix3f m1) { double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0); if (ww >= 0) { if (ww >= EPS2) { this.w = System.Math.Sqrt(ww); ww = 0.25 / this.w; this.x = ((m1.m21 - m1.m12) * ww); this.y = ((m1.m02 - m1.m20) * ww); this.z = ((m1.m10 - m1.m01) * ww); return; } } else { this.w = 0; this.x = 0; this.y = 0; this.z = 1; return; } this.w = 0; ww = (-0.5) * (m1.m11 + m1.m22); if (ww >= 0) { if (ww >= EPS2) { this.x = System.Math.Sqrt(ww); ww = 0.5 / this.x; this.y = (m1.m10 * ww); this.z = (m1.m20 * ww); return; } } else { this.x = 0; this.y = 0; this.z = 1; return; } this.x = 0; ww = 0.5 * (1.0 - m1.m22); if (ww >= EPS2) { this.y = System.Math.Sqrt(ww); this.z = (m1.m21 / (2.0 * this.y)); } this.y = 0; this.z = 1; }
/// <summary> /// Set a uniform mat3 in the shader. /// Uses a cached float[] to reduce memory usage. /// </summary> /// <param name="location">The location of the uniform in the shader.</param> /// <param name="param">The Matrix3f to load into the shader uniform.</param> public static void UniformMatrix3(int location, Matrix3f param) { // use the statically allocated float[] for setting the uniform matrix3Float[0] = param[0].X; matrix3Float[1] = param[0].Y; matrix3Float[2] = param[0].Z; matrix3Float[3] = param[1].X; matrix3Float[4] = param[1].Y; matrix3Float[5] = param[1].Z; matrix3Float[6] = param[2].X; matrix3Float[7] = param[2].Y; matrix3Float[8] = param[2].Z; GL.UniformMatrix3fv(location, 1, false, matrix3Float); }
public void SetUniform(string name, Matrix3f data) { int location = _getUniformLocation(name); if (location >= 0 && _checkCache(name, data)) { GL.UniformMatrix3(location, data); } }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. Vector3f offsetA, offsetB; Vector3f.Transform(ref LocalAnchorA, ref ConnectionA.Orientation, out offsetA); Vector3f.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); Vector3f anchorA, anchorB; Vector3f.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3f.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. Vector3f separation; Vector3f.Subtract(ref anchorB, ref anchorA, out separation); float currentDistance = separation.Length; //Compute jacobians Vector3f linearA; #if !WINDOWS linearA = new Vector3f(); #endif if (currentDistance > MathHelper.Epsilon) { linearA.X = separation.X / currentDistance; linearA.Y = separation.Y / currentDistance; linearA.Z = separation.Z / currentDistance; velocityBias = new Vector3f(errorCorrectionFactor * (currentDistance - distance), 0, 0); } else { velocityBias = new Vector3f(); linearA = new Vector3f(); } Vector3f angularA, angularB; Vector3f.Cross(ref offsetA, ref linearA, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3f.Cross(ref linearA, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3f { M11 = linearA.X, M12 = linearA.Y, M13 = linearA.Z }; linearJacobianB = new Matrix3f { M11 = -linearA.X, M12 = -linearA.Y, M13 = -linearA.Z }; angularJacobianA = new Matrix3f { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3f { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
///<summary> /// Constructs a new affine transform. ///</summary> ///<param name="scaling">Scaling to apply in the linear transform.</param> ///<param name="orientation">Orientation to apply in the linear transform.</param> ///<param name="translation">Translation to apply.</param> public AffineTransform(ref Vector3f scaling, ref Quaternion orientation, ref Vector3f translation) { //Create an SRT transform. Matrix3f.CreateScale(ref scaling, out LinearTransform); Matrix3f rotation; Matrix3f.FromQuaternion(ref orientation, out rotation); Matrix3f.Multiply(ref LinearTransform, ref rotation, out LinearTransform); Translation = translation; }
/// <summary> /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// </summary> /// <param name="matrix">Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis.</param> public void SetLocalAxes(Matrix3f matrix) { if (Math.Abs(Vector3f.Dot(matrix.Backward, matrix.Right)) > MathHelper.BigEpsilon) { throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); } localPrimaryAxis = Vector3f.Normalize(matrix.Backward); localXAxis = Vector3f.Normalize(matrix.Right); ComputeWorldSpaceAxes(); }
/// <summary> /// Updates the world inertia tensor based upon the local inertia tensor and current orientation. /// </summary> internal void UpdateInertiaTensor() { //This is separate from the position update because the orientation can change outside of our iteration loop, so this has to run first. //Iworld^-1 = RT * Ilocal^1 * R Matrix3f orientationMatrix; Matrix3f.FromQuaternion(ref Orientation, out orientationMatrix); Matrix3f.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out inertiaTensorInverse); Matrix3f.Multiply(ref inertiaTensorInverse, ref orientationMatrix, out inertiaTensorInverse); }
/// <summary> /// Performs any per-frame computation needed by the constraint. /// </summary> /// <param name="dt">Time step duration.</param> public override void Update(float dt) { //Collect references, pick the mode, and configure the coefficients to be used by the solver. if (supportData.SupportObject != null) { //Get an easy reference to the support. var support = supportData.SupportObject as EntityCollidable; if (support != null) { supportEntity = support.Entity; } else { supportEntity = null; } } else { supportEntity = null; } maximumForce = maximumGlueForce * dt; //If we don't allow the character to get out of the ground, it could apply some significant forces to a dynamic support object. //Let the character escape penetration in a controlled manner. This mirrors the regular penetration recovery speed. //Since the vertical motion constraint works in the opposite direction of the contact penetration constraint, //this actually eliminates the 'bounce' that can occur with non-character objects in deep penetration. permittedVelocity = Math.Min(Math.Max(supportData.Depth * CollisionResponseSettings.PenetrationRecoveryStiffness / dt, 0), CollisionResponseSettings.MaximumPenetrationRecoverySpeed); //Compute the jacobians and effective mass matrix. This constraint works along a single degree of freedom, so the mass matrix boils down to a scalar. linearJacobianA = supportData.Normal; Vector3f.Negate(ref linearJacobianA, out linearJacobianB); float inverseEffectiveMass = characterBody.InverseMass; if (supportEntity != null) { Vector3f offsetB = supportData.Position - supportEntity.Position; Vector3f.Cross(ref offsetB, ref linearJacobianB, out angularJacobianB); if (supportEntity.IsDynamic) { //Only dynamic entities can actually contribute anything to the effective mass. //Kinematic entities have infinite mass and inertia, so this would all zero out. Matrix3f inertiaInverse = supportEntity.InertiaTensorInverse; Vector3f angularComponentB; Vector3f.Transform(ref angularJacobianB, ref inertiaInverse, out angularComponentB); float effectiveMassContribution; Vector3f.Dot(ref angularComponentB, ref angularJacobianB, out effectiveMassContribution); inverseEffectiveMass += supportForceFactor * (effectiveMassContribution + supportEntity.InverseMass); } } effectiveMass = 1f / (inverseEffectiveMass); //So much nicer and shorter than the horizontal constraint! }
/// <summary> /// Multiplies a transform by another transform. /// </summary> /// <param name="a">First transform.</param> /// <param name="b">Second transform.</param> /// <param name="transform">Combined transform.</param> public static void Multiply(ref AffineTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3f linearTransform;//Have to use temporary variable just in case a or b reference is transform. Matrix3f.Multiply(ref a.LinearTransform, ref b.LinearTransform, out linearTransform); Vector3f translation; Vector3f.Transform(ref a.Translation, ref b.LinearTransform, out translation); Vector3f.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; }
/// <summary> /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// </summary> /// <param name="matrix">Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis.</param> public void SetWorldAxes(Matrix3f matrix) { if (Math.Abs(Vector3f.Dot(matrix.Backward, matrix.Right)) > MathHelper.BigEpsilon) { throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); } primaryAxis = Vector3f.Normalize(matrix.Backward); xAxis = Vector3f.Normalize(matrix.Right); Vector3f.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis); Vector3f.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis); }
/// <summary> Performs SVD on this matrix and gets the scale and the pure rotation. /// The pure rotation is placed into rot. /// </summary> /// <param name="rot">the rotation factor. /// </param> /// <returns> scale factor /// </returns> private float SVD(Matrix3f rot) { // this is a simple svd. // Not complete but fast and reasonable. // See comment in Matrix3d. double s = System.Math.Sqrt((m00 * m00 + m10 * m10 + m20 * m20 + m01 * m01 + m11 * m11 + m21 * m21 + m02 * m02 + m12 * m12 + m22 * m22) / 3.0); // zero-div may occur. double t = (s == 0.0?0.0:1.0 / s); if (rot != null) { this.getRotationScale(rot); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" rot.mul((float) t); } //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" return (float) s; }
/// <summary> Constructs and initializes a Matrix4d from the rotation matrix, /// translation, and scale values; the scale is applied only to the /// rotational components of the matrix (upper 3x3) and not to the /// translational components. /// </summary> /// <param name="m1"> The rotation matrix representing the rotational components /// </param> /// <param name="t1"> The translational components of the matrix /// </param> /// <param name="s"> The scale value applied to the rotational components /// </param> public Matrix4d(Matrix3f m1, Vector3d t1, double s) { // why no set(Matrix3f, Vector3d, double) ? // set(Matrix3f, Vector3f, float) is there. // feel inconsistent. set_Renamed(m1); mulRotationScale(s); setTranslation(t1); m33 = 1.0; }
/// <summary> Performs an SVD normalization of this matrix to calculate the rotation /// as a 3x3 matrix, the translation, and the scale. None of the matrix values are modified. /// </summary> /// <param name="m1">The normalized matrix representing the rotation /// </param> /// <param name="t1">The translation component /// </param> /// <returns> The scale component of this transform /// </returns> public double get_Renamed(Matrix3f m1, Vector3d t1) { get_Renamed(t1); return SVD(m1); }
/// <summary> Sets the value of this axis-angle to the rotational component of /// the passed matrix. /// If the specified matrix has no rotational component, the value /// of this AxisAngle4f is set to an angle of 0 about an axis of (0,1,0). /// </summary> /// <param name="m1">the matrix3f /// </param> public void set_Renamed(Matrix3f m1) { x = (float) (m1.m21 - m1.m12); y = (float) (m1.m02 - m1.m20); z = (float) (m1.m10 - m1.m01); double mag = x * x + y * y + z * z; if (mag > EPS) { mag = System.Math.Sqrt(mag); double sin = 0.5 * mag; double cos = 0.5 * (m1.m00 + m1.m11 + m1.m22 - 1.0); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" angle = (float) System.Math.Atan2(sin, cos); double invMag = 1.0 / mag; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" x = (float) (x * invMag); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" y = (float) (y * invMag); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" z = (float) (z * invMag); } else { x = 0.0f; y = 1.0f; z = 0.0f; angle = 0.0f; } }
/// <summary> Gets the upper 3x3 values of this matrix and places them into the matrix m1.</summary> /// <param name="m1">The matrix that will hold the values /// </param> public void getRotationScale(Matrix3f m1) { //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m00 = (float) m00; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m01 = (float) m01; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m02 = (float) m02; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m10 = (float) m10; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m11 = (float) m11; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m12 = (float) m12; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m20 = (float) m20; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m21 = (float) m21; //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" m1.m22 = (float) m22; }
/// <summary> Sets the value of this matrix from the rotation expressed by the /// rotation matrix m1, the translation t1, and the scale s. The translation /// is not modified by the scale. /// </summary> /// <param name="m1">The rotation component /// </param> /// <param name="t1">The translation component /// </param> /// <param name="scale">The scale component /// </param> public void set_Renamed(Matrix3f m1, Vector3f t1, float scale) { setRotationScale(m1); mulRotationScale(scale); setTranslation(t1); m33 = 1.0f; }
/// <summary> Performs an SVD normalization of this matrix in order to acquire the /// normalized rotational component; the values are placed into the Matrix3f parameter. /// </summary> /// <param name="m1">matrix into which the rotational component is placed /// </param> public void get_Renamed(Matrix3f m1) { SVD(m1, null); }
public void render1(Graphics g, Matrix3f matrixRotate, bool antialias, int x, int y) { //g3d.beginRendering(rectClip.X, rectClip.Y, rectClip.Width, rectClip.Height, matrixRotate, antialias); //repaintManager.render(g3d, rectClip, modelManager.Frame, repaintManager.displayModelIndex); //// mth 2003-01-09 Linux Sun JVM 1.4.2_02 //// Sun is throwing a NullPointerExceptions inside graphics routines //// while the window is resized. //g3d.endRendering(); //System.Drawing.Image img = g3d.ScreenImage; //try //{ // //UPGRADE_WARNING: Method 'java.awt.Graphics.drawImage' was converted to 'System.Drawing.Graphics.drawImage' which may throw an exception. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1101'" // g.DrawImage(img, x, y); //} //catch (System.NullReferenceException npe) //{ // System.Console.Out.WriteLine("Sun!! ... fix graphics your bugs!"); //} //g3d.releaseScreenImage(); }
/// <summary> Sets the rotational component (upper 3x3) of this matrix to the matrix /// values in the single precision Matrix3f argument; the other elements of /// this matrix are unchanged; a singular value decomposition is performed /// on this object's upper 3x3 matrix to factor out the scale, then this /// object's upper 3x3 matrix components are replaced by the passed rotation /// components, and then the scale is reapplied to the rotational /// components. /// </summary> /// <param name="m1">single precision 3x3 matrix /// </param> public void setRotation(Matrix3f m1) { double scale = SVD(null, null); setRotationScale(m1); mulRotationScale(scale); }
/// <summary> Sets the value of this matrix to the double value of the Matrix3f /// argument. /// </summary> /// <param name="m1">the matrix3d to be converted to double /// </param> public void set_Renamed(Matrix3f m1) { this.m00 = m1.m00; this.m01 = m1.m01; this.m02 = m1.m02; this.m10 = m1.m10; this.m11 = m1.m11; this.m12 = m1.m12; this.m20 = m1.m20; this.m21 = m1.m21; this.m22 = m1.m22; }
/// <summary> Replaces the upper 3x3 matrix values of this matrix with the values in the matrix m1.</summary> /// <param name="m1">The matrix that will be the new upper 3x3 /// </param> public void setRotationScale(Matrix3f m1) { m00 = m1.m00; m01 = m1.m01; m02 = m1.m02; m10 = m1.m10; m11 = m1.m11; m12 = m1.m12; m20 = m1.m20; m21 = m1.m21; m22 = m1.m22; }
/// <summary> Performs an SVD normalization of this matrix to calculate the rotation /// as a 3x3 matrix, the translation, and the scale. None of the matrix values are modified. /// </summary> /// <param name="m1">The normalized matrix representing the rotation /// </param> /// <param name="t1">The translation component /// </param> /// <returns> The scale component of this transform /// </returns> public float get_Renamed(Matrix3f m1, Vector3f t1) { get_Renamed(t1); return SVD(m1, null); }
/// <summary> Sets the rotational component (upper 3x3) of this matrix to the matrix /// values in the single precision Matrix3f argument; the other elements of /// this matrix are initialized as if this were an identity matrix /// (ie, affine matrix with no translational component). /// </summary> /// <param name="m1">the 3x3 matrix /// </param> public void set_Renamed(Matrix3f m1) { m00 = m1.m00; m01 = m1.m01; m02 = m1.m02; m03 = 0.0f; m10 = m1.m10; m11 = m1.m11; m12 = m1.m12; m13 = 0.0f; m20 = m1.m20; m21 = m1.m21; m22 = m1.m22; m23 = 0.0f; m30 = 0.0f; m31 = 0.0f; m32 = 0.0f; m33 = 1.0f; }
/// <summary> Performs SVD on this matrix and gets scale and rotation. /// Rotation is placed into rot. /// </summary> /// <param name="rot3">the rotation factor(Matrix3d). /// </param> /// <param name="rot4">the rotation factor(Matrix4f) only upper 3x3 elements are changed. /// </param> /// <returns> scale factor /// </returns> private float SVD(Matrix3f rot3, Matrix4f rot4) { // this is a simple svd. // Not complete but fast and reasonable. // See comment in Matrix3d. //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float s = (float) System.Math.Sqrt((m00 * m00 + m10 * m10 + m20 * m20 + m01 * m01 + m11 * m11 + m21 * m21 + m02 * m02 + m12 * m12 + m22 * m22) / 3.0); // zero-div may occur. float t = (s == 0.0f?0.0f:1.0f / s); if (rot3 != null) { this.getRotationScale(rot3); rot3.mul(t); } if (rot4 != null) { if (rot4 != this) rot4.setRotationScale(this); // private method rot4.mulRotationScale(t); // private method } return s; }
/// <summary> Constructs and initializes a Matrix4f from the rotation matrix, /// translation, and scale values; the scale is applied only to the /// rotational components of the matrix (upper 3x3) and not to the /// translational components. /// </summary> /// <param name="m1"> The rotation matrix representing the rotational components /// </param> /// <param name="t1"> The translational components of the matrix /// </param> /// <param name="s"> The scale value applied to the rotational components /// </param> public Matrix4f(Matrix3f m1, Vector3f t1, float s) { set_Renamed(m1); mulRotationScale(s); setTranslation(t1); m33 = 1.0f; }
/// <summary> Constructs a new matrix with the same values as the /// Matrix3f parameter. /// </summary> /// <param name="m1"> the source matrix /// </param> public Matrix3d(Matrix3f m1) { this.m00 = m1.m00; this.m01 = m1.m01; this.m02 = m1.m02; this.m10 = m1.m10; this.m11 = m1.m11; this.m12 = m1.m12; this.m20 = m1.m20; this.m21 = m1.m21; this.m22 = m1.m22; }
public void calcNotionalMatrix(float[] notionalUnitcell, Matrix3f matrixNotional) { // note that these are oriented as columns, not as row // this is because we will later use the transform method, // which multiplies the matrix by the point, not the point by the matrix float gamma = notionalUnitcell[5]; float beta = notionalUnitcell[4]; float alpha = notionalUnitcell[3]; float c = notionalUnitcell[2]; float b = notionalUnitcell[1]; float a = notionalUnitcell[0]; /* some intermediate variables */ //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float cosAlpha = (float) System.Math.Cos(toRadians * alpha); //float sinAlpha = (float)Math.sin(toRadians * alpha); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float cosBeta = (float) System.Math.Cos(toRadians * beta); //float sinBeta = (float)Math.sin(toRadians * beta); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float cosGamma = (float) System.Math.Cos(toRadians * gamma); //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float sinGamma = (float) System.Math.Sin(toRadians * gamma); // 1. align the a axis with x axis matrixNotional.setColumn(0, a, 0, 0); // 2. place the b is in xy plane making a angle gamma with a matrixNotional.setColumn(1, b * cosGamma, b * sinGamma, 0); // 3. now the c axis, // http://server.ccl.net/cca/documents/molecular-modeling/node4.html //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" float V = a * b * c * (float) System.Math.Sqrt(1.0 - cosAlpha * cosAlpha - cosBeta * cosBeta - cosGamma * cosGamma + 2.0 * cosAlpha * cosBeta * cosGamma); matrixNotional.setColumn(2, c * cosBeta, c * (cosAlpha - cosBeta * cosGamma) / sinGamma, V / (a * b * sinGamma)); }
public void getRotation(Matrix3f matrixRotation) { //transformManager.getRotation(matrixRotation); }
public void constructFractionalMatrices() { matrixNotional = new Matrix3f(); calcNotionalMatrix(notionalUnitcell, matrixNotional); if (pdbScaleMatrix != null) { // System.out.println("using PDB Scale matrix"); matrixEuclideanToFractional = new Matrix3f(); matrixEuclideanToFractional.transpose(pdbScaleMatrix); matrixFractionalToEuclidean = new Matrix3f(); matrixFractionalToEuclidean.invert(matrixEuclideanToFractional); } else { // System.out.println("using notional unit cell"); matrixFractionalToEuclidean = matrixNotional; matrixEuclideanToFractional = new Matrix3f(); matrixEuclideanToFractional.invert(matrixFractionalToEuclidean); } }
/// <summary> Sets the value of this quaternion to the rotational component of /// the passed matrix. /// </summary> /// <param name="m1">the Matrix3f /// </param> public void set_Renamed(Matrix3f m1) { float ww = 0.25f * (m1.m00 + m1.m11 + m1.m22 + 1.0f); if (ww >= 0) { if (ww >= EPS2) { //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" this.w = (float) System.Math.Sqrt((double) ww); ww = 0.25f / this.w; this.x = (m1.m21 - m1.m12) * ww; this.y = (m1.m02 - m1.m20) * ww; this.z = (m1.m10 - m1.m01) * ww; return ; } } else { this.w = 0; this.x = 0; this.y = 0; this.z = 1; return ; } this.w = 0; ww = (- 0.5f) * (m1.m11 + m1.m22); if (ww >= 0) { if (ww >= EPS2) { //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" this.x = (float) System.Math.Sqrt((double) ww); ww = 0.5f / this.x; this.y = m1.m10 * ww; this.z = m1.m20 * ww; return ; } } else { this.x = 0; this.y = 0; this.z = 1; return ; } this.x = 0; ww = 0.5f * (1.0f - m1.m22); if (ww >= EPS2) { //UPGRADE_WARNING: Data types in Visual C# might be different. Verify the accuracy of narrowing conversions. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1042'" this.y = (float) System.Math.Sqrt((double) ww); this.z = m1.m21 / (2.0f * this.y); return ; } this.y = 0; this.z = 1; }
/// <summary> Sets the value of this quaternion to the rotational component of /// the passed matrix. /// </summary> /// <param name="m1">the matrix3f /// </param> public void set_Renamed(Matrix3f m1) { double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0); if (ww >= 0) { if (ww >= EPS2) { this.w = System.Math.Sqrt(ww); ww = 0.25 / this.w; this.x = ((m1.m21 - m1.m12) * ww); this.y = ((m1.m02 - m1.m20) * ww); this.z = ((m1.m10 - m1.m01) * ww); return ; } } else { this.w = 0; this.x = 0; this.y = 0; this.z = 1; return ; } this.w = 0; ww = (- 0.5) * (m1.m11 + m1.m22); if (ww >= 0) { if (ww >= EPS2) { this.x = System.Math.Sqrt(ww); ww = 0.5 / this.x; this.y = (m1.m10 * ww); this.z = (m1.m20 * ww); return ; } } else { this.x = 0; this.y = 0; this.z = 1; return ; } this.x = 0; ww = 0.5 * (1.0 - m1.m22); if (ww >= EPS2) { this.y = System.Math.Sqrt(ww); this.z = (m1.m21 / (2.0 * this.y)); } this.y = 0; this.z = 1; }
/// <summary> Sets the rotational component (upper 3x3) of this matrix to the matrix /// values in the single precision Matrix3f argument; the other elements of /// this matrix are unchanged; a singular value decomposition is performed /// on this object's upper 3x3 matrix to factor out the scale, then this /// object's upper 3x3 matrix components are replaced by the passed rotation /// components, and then the scale is reapplied to the rotational /// components. /// </summary> /// <param name="m1">single precision 3x3 matrix /// </param> public void setRotation(Matrix3f m1) { float scale = SVD(null); setRotationScale(m1); mulRotationScale(scale); }
public Matrix3f GetMatrix3f() { Matrix3f result = new Matrix3f(); unsafe { Get(&result.XX, new Vector2i(3, 3), 1); } return result; }