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