public static float GetRotation(ref Matrix matrix)
{
Vector3 position3, scale3;
Quaternion rotationQ;
matrix.Decompose(out scale3, out rotationQ, out position3);
Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
return (float)Math.Atan2(direction.Y, direction.X);
}
/// <summary>
/// Construct this transform from a matrix
/// </summary>
/// <param name="matrix"></param>
/// <param name="performValidityCheck">When true, the matrix will be checked to make sure it will produce a valid transform</param>
public Transform(ref Matrix matrix, bool performValidityCheck)
{
if (performValidityCheck)
{
//validate the matrix is not sheered or inverted on a single axis.
Vector3 x = new Vector3(matrix.M11, matrix.M12, matrix.M13);
Vector3 y = new Vector3(matrix.M21, matrix.M22, matrix.M23);
Vector3 z = new Vector3(matrix.M31, matrix.M32, matrix.M33);
float xl = x.Length();
float yl = y.Length();
float zl = z.Length();
float maxl = Math.Max(xl, Math.Max(yl, zl));
float minl = Math.Min(xl, Math.Min(yl, zl));
if ((maxl - minl) > maxl / 10)
throw new ArgumentException("The input matrix is not uniformly scaled");
if (xl > 0.000001f) x /= xl;
if (yl > 0.000001f) y /= yl;
if (zl > 0.000001f) z /= zl;
Vector3 zc = Vector3.Cross(x, y);
Vector3 yc = Vector3.Cross(z, x);
Vector3 xc = Vector3.Cross(y, zc);
if (Vector3.Dot(x, xc) < 0.975f || Vector3.Dot(z, zc) * Vector3.Dot(y, yc) < 0.95f)
throw new ArgumentException("The input matrix is skewed, sheered or non uniformly scaled");
}
Vector3 scale;
matrix.Decompose(out scale, out Rotation, out Translation);
//if one or two components are negative, then the Decompose messed up.
if (scale.X * scale.Y * scale.Z < 0)
{
Matrix copy = matrix;
copy.M11 = -copy.M11;
copy.M12 = -copy.M12;
copy.M13 = -copy.M13;
copy.M21 = -copy.M21;
copy.M22 = -copy.M22;
copy.M23 = -copy.M23;
copy.M31 = -copy.M31;
copy.M32 = -copy.M32;
copy.M33 = -copy.M33;
copy.Decompose(out scale, out Rotation, out Translation);
scale = -scale;
}
this.Scale = Math.Min(Math.Min(scale.X, scale.Y), scale.Z);
if (Scale > 0.999f && Scale < 1.001f)
Scale = 1;
this.Rotation.Normalize();
}
public static void DecomposeMatrix2D(ref Matrix matrix, out Vector2 position, out float rotation, out Vector2 scale)
{
Vector3 position3, scale3;
Quaternion rotationQ;
matrix.Decompose(out scale3, out rotationQ, out position3);
Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
rotation = (float)Math.Atan2(direction.Y, direction.X);
position = new Vector2(position3.X, position3.Y);
scale = new Vector2(scale3.X, scale3.Y);
}
public virtual void SetTransform(Matrix m)
{
Vector3 pos;
Quaternion quat;
Vector3 scale;
m.Decompose(out scale,out quat,out pos);
Vector3 rot = MathUtils.QuaternionToEuler(quat);
transform.Set(new Vector2(pos.X,pos.Y), (float)rot.Z);
SetScale(new Vector2(scale.X,scale.Y));
}
/// <summary>
/// Construct this transform from a matrix
/// </summary>
/// <param name="matrix"></param>
public Transform(ref Matrix matrix)
{
Vector3 scale;
matrix.Decompose(out scale, out Rotation, out Translation);
this.Scale = Math.Min(Math.Min(scale.X, scale.Y), scale.Z);
if (Scale > 0.9999f && Scale < 1.0001f)
Scale = 1;
}
public void SetTransform(ref Matrix transform)
{
transform.Decompose(out scale, out rotation, out translation);
if (scale.X > 0.9999f && scale.X <= 1.0001f)
scale.X = 1;
if (scale.Y > 0.9999f && scale.Y <= 1.0001f)
scale.Y = 1;
if (scale.Z > 0.9999f && scale.Z <= 1.0001f)
scale.Z = 1;
}
// Converts a Rotation Matrix to a quaternion, then into a Vector3 containing
// Euler angles (X: Pitch, Y: Yaw, Z: Roll)
public Vector3 MatrixToEulerAngleVector3(Matrix Rotation)
{
Vector3 translation, scale;
Quaternion rotation;
Rotation.Decompose(out scale, out rotation, out translation);
Vector3 eulerVec = QuaternionToEulerAngleVector3(rotation);
return eulerVec;
}
/// <summary>
/// 指定された行列から生成する
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static SQTTransformContent FromMatrix(Matrix matrix)
{
// 行列の分解
Quaternion rotation;
Vector3 translation;
Vector3 scale;
matrix.Decompose(out scale, out rotation, out translation);
return new SQTTransformContent(scale, rotation, translation);
}
/// <summary>
/// Creates a new keyframe from given time and transformation matrix.
/// Since internally the joint transformation is stored in form of its
/// scale, rotation and translation components the matrix has to be
/// decomposed within this constructor.
/// </summary>
/// <param name="time">Time of keyframe</param>
/// <param name="transform">Transformation</param>
public JointAnimationKeyFrame(float time, Matrix transform)
{
_time = time;
if (!transform.Decompose(out _scale, out _rotation, out _translation))
{
throw new ApplicationException("Could not decompose transformation matrix");
}
_transform = transform;
}
public static BoundingSphere Transform(this BoundingSphere source, Matrix transformation)
{
Vector3 translation;
Vector3 scaling;
Quaternion rotation;
transformation.Decompose(out scaling, out rotation, out translation);
var transformedSphereRadius = source.Radius * new[] { scaling.X, scaling.Y, scaling.Z }.Max();
var transformedSphereCenter = Vector3.Transform(source.Center, transformation);
return new BoundingSphere(transformedSphereCenter, transformedSphereRadius);
}
public static string GetInfo(Matrix matrix)
{
Vector3 scales;
Quaternion quaternion;
Vector3 translation;
matrix.Decompose(out scales, out quaternion, out translation);
return string.Format("Scales:{0}; Angles:{1}; Trans:{2}", scales,
//GetAngles(matrix),
QuaternionToEuler2(quaternion),
translation);
}
public void DrawShield(Matrix world)
{
Vector3 scale, translation;
Quaternion rotation;
//should actually be up...
Vector3 side = world.Right;
Vector3 forward = world.Up;
world.Decompose(out scale, out rotation, out translation);
scale.Z *= .8f;
scale *= 1.5f;
// scale.X *= 3f;
//translation -= world.Right;
rotation = Quaternion.Concatenate(rotation, Quaternion.CreateFromAxisAngle(world.Right, MathHelper.ToRadians(90)));
Matrix.CreateFromQuaternion(ref rotation, out world);
world = Matrix.CreateScale(scale) * world;
world *= Matrix.CreateTranslation(translation);
// world = Matrix.CreateScale(scale*2) * Matrix.CreateFromQuaternion(rotation) * Matrix.CreateRotationZ((float)(Math.PI / 2)) * Matrix.CreateTranslation(translation);
Matrix[] ModelTransforms = new Matrix[shieldModel.Bones.Count];
shieldModel.CopyAbsoluteBoneTransformsTo(ModelTransforms);
foreach (ModelMesh mesh in shieldModel.Meshes)
{
foreach (Effect currentEffect in mesh.Effects)
{
currentEffect.CurrentTechnique = currentEffect.Techniques["Colored"];
currentEffect.Parameters["xWorld"].SetValue(ModelTransforms[mesh.ParentBone.Index] * world);
currentEffect.Parameters["xView"].SetValue(viewMatrix);
currentEffect.Parameters["xProjection"].SetValue(projectionMatrix);
Vector3 lightDirection = new Vector3(3, -1, 0);
lightDirection.Normalize();
currentEffect.Parameters["xLightDirection"].SetValue(lightDirection);
currentEffect.Parameters["xShieldP1"].SetValue(P1shieldDirection);
currentEffect.Parameters["xShieldP2"].SetValue(P2shieldDirection);
currentEffect.Parameters["xShipSide"].SetValue(side);
currentEffect.Parameters["xShipForward"].SetValue(forward);
// currentEffect.Parameters["xTexture"].SetValue(texture);
}
mesh.Draw();
}
}
public Transform(Matrix matrix)
{
matrix.Decompose(out scale, out rotation, out translation);
if (scale.X > 0.9999f && scale.X <= 1.0001f)
scale.X = 1;
if (scale.Y > 0.9999f && scale.Y <= 1.0001f)
scale.Y = 1;
if (scale.Z > 0.9999f && scale.Z <= 1.0001f)
scale.Z = 1;
#if DEBUG
this.Validate();
#endif
}
public static BoundingBox TransformBox(BoundingBox aabb, Matrix transform)
{
Vector3 scale;
Quaternion rotation;
Vector3 translation;
transform.Decompose(out scale, out rotation, out translation);
if (rotation != Quaternion.Identity)
throw new ArgumentException("Rotation of AABBs is not supported.");
Vector3 center = 0.5f * (aabb.Max + aabb.Min) + translation;
Vector3 extent = 0.5f * (aabb.Max - aabb.Min) * scale;
//return new BoundingBox(center - extent, center + extent);
return BoundingBox.CreateFromPoints(new Vector3[] { center - extent, center + extent });
}
/// <summary>
/// 指定された行列から生成する
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static QuatTransform FromMatrix( Matrix matrix )
{
// 行列の分解
Quaternion rotation;
Vector3 translation;
Vector3 scale;
matrix.Decompose( out scale, out rotation, out translation );
// 一意のスケールか？
if ( !CloseEnough( scale.X, scale.Y ) || !CloseEnough( scale.X, scale.Z ) )
{
throw new InvalidOperationException(
"一意のスケール(X,Y,Zが同じスケール値)ではありません" );
}
if ( !CloseEnough( scale.X, 1.0f ) )
throw new InvalidOperationException( "スケール値が1以外です" );
return new QuatTransform( rotation, translation );
}
public static BoundingSphere TransformBoundingSphere(BoundingSphere originalBoundingSphere, Matrix transformationMatrix)
{
Vector3 trans;
Vector3 scaling;
Quaternion rot;
transformationMatrix.Decompose(out scaling, out rot, out trans);
float maxScale = scaling.X;
if (maxScale < scaling.Y)
maxScale = scaling.Y;
if (maxScale < scaling.Z)
maxScale = scaling.Z;
float transformedSphereRadius = originalBoundingSphere.Radius * maxScale;
Vector3 transformedSphereCenter = Vector3.Transform(originalBoundingSphere.Center, transformationMatrix);
BoundingSphere transformedBoundingSphere = new BoundingSphere(transformedSphereCenter, transformedSphereRadius);
return transformedBoundingSphere;
}
/// <summary>
/// DecomposeMatRot decomposes a matrix into its component parts and return the quaternion rotation.
/// </summary>
/// <param name="inMat">Matrix to decompose.</param>
/// <returns>Returns a quaternion rotation from the Matrix.</returns>
public static Quaternion DecomposeMatRot(Matrix inMat)
{
Quaternion rot;
Vector3 scale;
Vector3 trans;
inMat.Decompose(out scale, out rot, out trans);
return rot;
}
public void GetSRT(Microsoft.Xna.Framework.Matrix m)
{
Microsoft.Xna.Framework.Vector3 scale = Microsoft.Xna.Framework.Vector3.Zero;
Microsoft.Xna.Framework.Vector3 translate = Microsoft.Xna.Framework.Vector3.Zero;
Microsoft.Xna.Framework.Quaternion q = Microsoft.Xna.Framework.Quaternion.Identity;
m.Decompose(out scale, out q, out translate);
this.tX = translate.X;
this.tY = translate.Y;
this.tZ = translate.Z;
this.sX = scale.X;
this.sY = scale.Y;
this.sZ = scale.Z;
float Singularity = 0.499f;
float ww = q.W * q.W;
float xx = q.X * q.X;
float yy = q.Y * q.Y;
float zz = q.Z * q.Z;
float lengthSqd = xx + yy + zz + ww;
float singularityTest = q.Y * q.W - q.X * q.Z;
float singularityValue = Singularity * lengthSqd;
if (singularityTest > singularityValue)
{
this.rX = (-2f * (float)Math.Atan2(q.Z, q.W));
this.rY = (90.0f) * (float)(Math.PI / 180);
this.rZ = (0f) * (float)(Math.PI / 180);
}
else
{
if (singularityTest < -singularityValue)
{
this.rX = (2 * (float)Math.Atan2(q.Z, q.W));
this.rY = (-90.0f) * (float)(Math.PI / 180);
this.rZ = (0f) * (float)(Math.PI / 180);
}
else
{
this.rX = ((float)Math.Atan2(2.0f * (q.Y * q.Z + q.X * q.W), 1.0f - 2.0f * (xx + yy)));
this.rY = ((float)Math.Asin(2.0f * singularityTest / lengthSqd));
this.rZ = ((float)Math.Atan2(2.0f * (q.X * q.Y + q.Z * q.W), 1.0f - 2.0f * (yy + zz)));
}
}
while (this.rX > Math.PI)
{
this.rX -= (float)(2 * Math.PI);
}
while (this.rX < -Math.PI)
{
this.rX += (float)(2 * Math.PI);
}
while (this.rY > Math.PI)
{
this.rY -= (float)(2 * Math.PI);
}
while (this.rY < -Math.PI)
{
this.rY += (float)(2 * Math.PI);
}
while (this.rZ > Math.PI)
{
this.rZ -= (float)(2 * Math.PI);
}
while (this.rZ < -Math.PI)
{
this.rZ += (float)(2 * Math.PI);
}
}
/// <summary>
/// Decomposes transform matrix to position, rotation and scale
/// </summary>
/// <param name="matrix">Matrix to decompose</param>
/// <param name="position">Resulting position</param>
/// <param name="rotation">Resulting rotation in radians</param>
/// <param name="scale">Resulting scale</param>
/// <returns>true if decomposition is possible, false otherwise</returns>
protected bool DecomposeMatrix(ref Matrix matrix, out Vector2 position, out float rotation, out Vector2 scale)
{
Vector3 position3, scale3;
Quaternion rotationQ;
position = Vector2.Zero; scale = Vector2.One; rotation = 0;
if (!matrix.Decompose(out scale3, out rotationQ, out position3))
{
return false;
}
Vector2 direction = Vector2.Transform(Vector2.UnitX, rotationQ);
rotation = (float)Math.Atan2(direction.Y, direction.X);
position = new Vector2(position3.X, position3.Y);
scale = new Vector2(scale3.X, scale3.Y);
return true;
}
private Matrix GetAverage(Matrix original)
{
List<Matrix> transforms = new List<Matrix>();
foreach (RelativeMarker marker in supportingMarkers)
{
if (marker.Node.MarkerFound && marker.Initialized)
{
transforms.Add(Matrix.Multiply(marker.RelativeTransform,
marker.Node.WorldTransformation));
}
}
if (transforms.Count > 0)
{
Vector3 origPos, temp;
Quaternion origRot;
original.Decompose(out temp, out origRot, out origPos);
Vector3 pos;
Quaternion rot;
foreach (Matrix mat in transforms)
{
mat.Decompose(out temp, out rot, out pos);
origPos += pos;
Quaternion.Lerp(ref origRot, ref rot, 1.0f / (1 + transforms.Count), out origRot);
}
origRot.Normalize();
origPos *= 1.0f / (1 + transforms.Count);
Matrix result = Matrix.CreateFromQuaternion(origRot);
result.Translation = origPos;
return result;
}
else
return original;
}
//----------------------------------------------------------------------//
// 関数名 TransformRootBone //
// Function name TransformRootBone
// 機能 ルートとなる関節に合わせてモデルのボーン情報を生成する //
// Create a bone model information in accordance with the joint to become the root function
// 引数 スケルトンデータ、ボーンデータ //
// Argument data skeleton, bone data
// 戻り値 なし //
// No return value
//----------------------------------------------------------------------//
private void TransformRootBone(Skeleton skeleton, ref Matrix boneTransform)
{
Vector3 s, t;
Quaternion r;
if (boneTransform.Decompose(out s, out r, out t))
{
// I get the coordinates of the joint on the left and right sides of the origin
var leftPosition = ConvertJointPosition(skeleton, JointType.HipLeft);
var rightPosition = ConvertJointPosition(skeleton, JointType.HipRight);
// I get the rotation axis and angle to rotate the bone from the difference of the coordinates
var direction = Vector3.Normalize(leftPosition - rightPosition);
var angle = (float)Math.Acos(Vector3.Dot(Vector3.Right, direction));
var axis = Vector3.Normalize(Vector3.Cross(Vector3.Right, direction));
// I want to convert to the amount of movement of the model the amount of movement of the origin
t = ConvertJointPosition(skeleton, JointType.HipCenter) * 1;
// Set the reference argument by generating bone information
boneTransform = Matrix.CreateScale(s) * Matrix.CreateFromQuaternion(r) * Matrix.CreateFromAxisAngle(axis, angle) * Matrix.CreateTranslation(t);
}
}
//------------------------------------------------------------------//
// 関数名 TransformNodeBone //
// Function name TransformNodeBone
// 機能 ノードとなる関節に合わせてモデルのボーン情報を生成 //
// Create a bone model information in accordance with the joint that it is functional nodes
// 引数 スケルトンデータ //
// Argument skeleton data
// 始点の関節 //
// Joint of the starting point
// 終点の関節 //
// End point of the point
// ボーン情報 //
// Bone Information
// 戻り値 なし //
// No return value
//------------------------------------------------------------------//
private void TransformNodeBone( Skeleton skeleton,
JointType beginJointType,
JointType endJointType,
ref Matrix boneTransform)
{
Vector3 scale, translation;
Quaternion rotation;
// Scale, rotation, broken down into the bone matrix position
// Decompose the function matrix is Decompose ()
if (boneTransform.Decompose(out scale, out rotation, out translation))
{
// I get the coordinates of joints and end joints of the start point
var beginPosition = ConvertJointPosition(skeleton, beginJointType);
var endPosition = ConvertJointPosition(skeleton, endJointType);
// I get the rotation axis and angle to rotate the bone from the difference of the coordinates
var direction = Vector3.Normalize(endPosition - beginPosition);
var angle = (float)Math.Acos(Vector3.Dot(Vector3.Down, direction));
var axis = Vector3.Normalize(Vector3.Cross(Vector3.Down, direction));
// I cut off the rotation of the parent bone
rotation = Quaternion.Identity;
// Set the reference argument by generating bone information
boneTransform = Matrix.CreateScale(scale)
* Matrix.CreateFromQuaternion(rotation)
* Matrix.CreateFromAxisAngle(axis, angle)
* Matrix.CreateTranslation(translation);
}
}
/// <summary>
/// Updates this node's gloabl matrix based on the parent's global matrix, which
/// is passed in and this node's local matrix
/// </summary>
/// <param name="parentMatrix"></param>
public virtual void UpdateGlobalMatrix(Matrix parentMatrix)
{
globalMatrix = localMatrix * parentMatrix;
inverseGlobalMatrix = Matrix.Invert(globalMatrix);
Quaternion quat;
Vector3 pos;
Vector3 scl;
globalMatrix.Decompose(out scl, out quat, out pos);
globalPosition = new Vector2(pos.X, pos.Y);
globalRotation = MathUtils.QuaternionToEuler(quat).Z;
globalScale = new Vector2(scl.X, scl.Y);
UpdateChildMatrices();
}
public virtual void Update(RenderContext renderContext)
{
WorldMatrix = Matrix.CreateFromQuaternion(LocalRotation) *
Matrix.CreateScale(LocalScale) *
Matrix.CreateTranslation(LocalPosition);
if (Parent != null)
{
WorldMatrix = Matrix.Multiply(WorldMatrix, Parent.WorldMatrix);
Vector3 scale, position;
Quaternion rotation;
if (!WorldMatrix.Decompose(out scale, out rotation, out position))
{
System.Diagnostics.Debug.WriteLine("Object3D Decompose World Matrix FAILED!");
}
WorldPosition = position;
WorldScale = scale;
WorldRotation = rotation;
}
else
{
WorldPosition = LocalPosition;
WorldScale = LocalScale;
WorldRotation = LocalRotation;
}
Children.ForEach(child => child.Update(renderContext));
if (_relativeBoundingBox.HasValue)
BoundingBox = _relativeBoundingBox.Value.Update(WorldMatrix);
}
public static void decomposeMatrix(ref Matrix matrix, out Vector2 position, out Vector2 scale)
{
matrix.Decompose(out scale3, out Node.RotationQuaternoin, out position3);
position.X = position3.X;
position.Y = position3.Y;
scale.X = scale3.X;
scale.Y = scale3.Y;
}
public void SetWorldMatrix(Matrix V)
{
fWorldMatrix = V;
Vector3 Scale;
Vector3 Translation;
fWorldMatrix.Decompose(out Scale, out fOrientation, out Translation);
fOrientation = Quaternion.Inverse(fOrientation);
// transform the local reference vectors to get the world vectors
Vector3.Transform(ref fForwardLocal, ref fOrientation, out fForward);
Vector3.Transform(ref fRightLocal, ref fOrientation, out fRight);
Vector3.Transform(ref fUpLocal, ref fOrientation, out fUp);
}
/// <summary>
/// Method to transform a bounding box's corners into world space
/// </summary>
/// <param name="input">untransformed bounding box</param>
/// <param name="world">world matrix</param>
/// <returns>transformed bounding box</returns>
public static BoundingSphere TransformBox(BoundingSphere input, Matrix world)
{
Vector3 scale = Vector3.Zero;
Vector3 translate = Vector3.Zero;
Quaternion rotation = Quaternion.Identity;
world.Decompose(out scale, out rotation, out translate);
float dist = Math.Max(input.Radius*scale.X,
Math.Max(input.Radius * scale.Y,
input.Radius*scale.Z));
return new BoundingSphere(Vector3.Transform(input.Center,world),
dist);
}
public void Decompose()
{
Matrix m01 = new Matrix(1f, 1f, 1f, 1f, -0.5f, 2.25f, 3.5f, 2f, 1f, -0.5f, -2f, 3.75f, 0, 3.5f, 2f, 1f);
Vector3 s01 = new Vector3();
Vector3 t01 = new Vector3();
Quaternion q01 = new Quaternion();
bool d01 = m01.Decompose(out s01, out q01, out t01);
Vector3 s02 = new Vector3(1.732051f, -4.190763f, 2.291288f);
Vector3 t02 = new Vector3(0f, 3.5f, 2f);
Quaternion q02 = new Quaternion(0f, 0f, 0f, 1f);
bool d02 = false;
Matrix tmp01 = Matrix.CreateFromQuaternion(new Quaternion(3f, 0, 0, -4f));
Matrix tmp02 = Matrix.CreateTranslation(5.25f, 3.336f, -2.95f);
Matrix tmp03 = Matrix.CreateScale(2.5f);
Matrix m11 = Matrix.Multiply(tmp01, tmp02);
m11 = Matrix.Multiply(m11, tmp03);
Vector3 s11 = new Vector3();
Vector3 t11 = new Vector3();
Quaternion q11 = new Quaternion();
bool d11 = m11.Decompose(out s11, out q11, out t11);
Vector3 s12 = new Vector3(2.5f, 73.52721f, 73.52721f);
Vector3 t12 = new Vector3(13.125f, 8.34f, -7.375f);
Quaternion q12 = new Quaternion(0.8882616f, 0f,0f, -0.4593379f);
bool d12 = true;
Assert.AreEqual(TestHelper.Approximate(s02), TestHelper.Approximate(s01), "#1");
Assert.AreEqual(TestHelper.Approximate(t02), TestHelper.Approximate(t01), "#2");
Assert.AreEqual(TestHelper.Approximate(q02), TestHelper.Approximate(q01), "#3");
Assert.AreEqual(d02, d01, "#4");
Assert.AreEqual(TestHelper.Approximate(s12), TestHelper.Approximate(s11), "#5");
Assert.AreEqual(TestHelper.Approximate(t12), TestHelper.Approximate(t11), "#6");
Assert.AreEqual(TestHelper.Approximate(q12), TestHelper.Approximate(q11), "#7");
Assert.AreEqual(d12, d11, "#8");
}
public void ModifyPhysicsObject(IPhysicsObject physObj, Matrix newTransform)
{
if (!objectIDs.ContainsKey(physObj))
return;
IntPtr body = objectIDs[physObj];
Quaternion rot;
Vector3 trans;
Vector3 scale;
newTransform.Decompose(out scale, out rot, out trans);
Matrix startTransform = Matrix.CreateFromQuaternion(rot);
if (physObj.Shape == ShapeType.Capsule ||
physObj.Shape == ShapeType.Cone ||
physObj.Shape == ShapeType.Cylinder ||
physObj.Shape == ShapeType.ChamferCylinder)
startTransform *= Matrix.CreateRotationZ(MathHelper.PiOver2);
startTransform.Translation = trans;
// if none of the physics properties are modified, then we only need to set the transform
if (!physObj.Modified && scaleTable[body].Equals(scale))
{
SetTransform(physObj, newTransform);
return;
}
if (!scaleTable[body].Equals(scale) || physObj.ShapeModified)
{
IntPtr collision = GetNewtonCollision(physObj, scale);
Newton.NewtonBodySetCollision(body, collision);
Newton.NewtonReleaseCollision(nWorld, collision);
scaleTable.Remove(body);
scaleTable.Add(body, scale);
physObj.ShapeModified = false;
}
// rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (physObj.Mass != 0.0f && physObj.Interactable);
physObj.PhysicsWorldTransform = newTransform;
// set the matrix for the rigid body
Newton.NewtonBodySetMatrix(body, MatrixHelper.ToFloats(startTransform));
if (isDynamic)
{
// set the transform call back function
// if the SetTransformCallback is specified by the programmer for this specific
// physics object, then use the specified one; otherwise, use the default
// SetTransformCallback function
if(setTransformMap.ContainsKey(physObj))
Newton.NewtonBodySetTransformCallback(body, setTransformMap[physObj]);
else
Newton.NewtonBodySetTransformCallback(body, transformCallback);
// set the force and torque call back function
// if the ApplyForceAndTorqueCallback is specified by the programmer for this specific
// physics object, then use the specified one; otherwise, use the default
// ApplyForceAndTorqueCallback function
if(applyForceMap.ContainsKey(physObj))
Newton.NewtonBodySetForceAndTorqueCallback(body, applyForceMap[physObj]);
else
Newton.NewtonBodySetForceAndTorqueCallback(body, applyForceAndTorqueCallback);
// set the mass matrix and moment of inertia
Vector3 momentOfInertia = physObj.MomentOfInertia;
if (momentOfInertia.Equals(Vector3.Zero))
{
IntPtr collision = Newton.NewtonBodyGetCollision(body);
momentOfInertia = GetMomentOfInertia(physObj, scale, collision);
}
// Swap the axis values since Newton's capsule, cone, and cylinder primitives are oriented
// along X axis while our primitives are oriented along Y axis
else if (physObj.Shape == ShapeType.Capsule ||
physObj.Shape == ShapeType.Cone ||
physObj.Shape == ShapeType.Cylinder ||
physObj.Shape == ShapeType.ChamferCylinder)
{
float tmp = momentOfInertia.Y;
momentOfInertia.Y = -momentOfInertia.X;
momentOfInertia.X = tmp;
}
Newton.NewtonBodySetMassMatrix(body, physObj.Mass, momentOfInertia.X,
momentOfInertia.Y, momentOfInertia.Z);
// set the center of mass if not new Vector3()
if (!physObj.CenterOfMass.Equals(Vector3.Zero))
{
float[] centerOfMass = {physObj.CenterOfMass.X, physObj.CenterOfMass.Y,
physObj.CenterOfMass.Z};
// Swap the axis values since Newton's capsule, cone, and cylinder primitives are oriented
// along X axis while our primitives are oriented along Y axis
if (physObj.Shape == ShapeType.Capsule ||
physObj.Shape == ShapeType.Cone ||
physObj.Shape == ShapeType.Cylinder ||
physObj.Shape == ShapeType.ChamferCylinder)
{
centerOfMass[0] = physObj.CenterOfMass.Y;
centerOfMass[1] = -physObj.CenterOfMass.X;
}
Newton.NewtonBodySetCentreOfMass(body, centerOfMass);
}
// set the initial force and torque
Newton.NewtonBodySetVelocity(body, Vector3Helper.ToFloats(physObj.InitialLinearVelocity));
Newton.NewtonBodySetOmega(body, Vector3Helper.ToFloats(physObj.InitialAngularVelocity));
// set the damping values
if (physObj.LinearDamping >= 0)
Newton.NewtonBodySetLinearDamping(body, physObj.LinearDamping);
if (physObj.AngularDamping != -Vector3.One)
{
float[] angularDamping = {physObj.AngularDamping.X, physObj.AngularDamping.Y,
physObj.AngularDamping.Z};
Newton.NewtonBodySetAngularDamping(body, angularDamping);
}
if (physObj.NeverDeactivate)
Newton.NewtonBodySetAutoFreeze(body, 0);
else
Newton.NewtonBodySetAutoFreeze(body, 1);
}
// Apply extra settings for vehicle joint
if (physObj is NewtonVehicle)
{
NewtonVehicle vehicle = (NewtonVehicle)physObj;
float[] upDir = Vector3Helper.ToFloats(startTransform.Up);
vehicle.Joint = Newton.NewtonConstraintCreateVehicle(nWorld, upDir, body);
if (vehicle.TransformCallback == null)
throw new GoblinException("You need to set the TransformCallback for NewtonVehicle");
if (vehicle.ForceCallback == null)
throw new GoblinException("You need to set the ForceCallback for NewtonVehicle");
if (vehicle.TireUpdateCallback == null)
throw new GoblinException("You need to set the TireUpdateCallback for NewtonVehicle");
Newton.NewtonBodySetTransformCallback(body, vehicle.TransformCallback);
Newton.NewtonBodySetForceAndTorqueCallback(body, vehicle.ForceCallback);
Newton.NewtonVehicleSetTireCallback(vehicle.Joint, vehicle.TireUpdateCallback);
for (int i = 0; i < 4; i++)
{
NewtonTire tire = vehicle.Tires[i];
if (tire == null)
throw new GoblinException("All of the four tires need to be set: " + ((TireID)i)
+ " tire is null");
float[] localMatrix = MatrixHelper.ToFloats(tire.TireOffsetMatrix);
float[] pin = Vector3Helper.ToFloats(tire.Pin);
IntPtr newtonTireID = Newton.NewtonVehicleAddTire(vehicle.Joint, localMatrix, pin,
tire.Mass, tire.Width, tire.Radius, tire.SuspensionShock, tire.SuspensionSpring,
tire.SuspensionLength, IntPtr.Zero, tire.CollisionID);
vehicle.AddToTireTable(newtonTireID, i);
}
}
if (!physObj.Collidable)
Newton.NewtonWorldFreezeBody(nWorld, body);
else
Newton.NewtonWorldUnfreezeBody(nWorld, body);
}
public static void Transform(Mesh mesh, Matrix m, int start, int count)
{
if (start < 0) start = mesh.verticies.Length - start;
for (int i = start; i < start + count; ++i)
mesh.verticies[i].Position = Vector3.Transform(mesh.verticies[i].Position, m);
Vector3 scale;
Vector3 trans;
Quaternion rot;
m.Decompose(out scale, out rot, out trans);
for (int i = start; i < start + count; ++i)
mesh.verticies[i].Normal = Vector3.Transform(mesh.verticies[i].Normal, rot);
}
/// <summary>
/// Update the transform by extracting the scale - rotation - position from a matrix
/// </summary>
/// <param name="matrix">Matrix used to update the transform</param>
public void UpdateFromMatrix(ref Matrix matrix)
{
matrix.Decompose(out LocalTransformScale, out LocalTransformRotation, out LocalTransformPosition);
RequireUpdate();
}