public void Add(float addMass, BXDVector3 addCOM)
{
this.centerOfMass.Multiply(mass);
this.centerOfMass.Add(addCOM.Copy().Multiply(addMass));
this.mass += addMass;
this.centerOfMass.Multiply(1.0f / this.mass);
}
/// <summary>
/// Reads a BXDVector3 with the given XmlReader and returns the reading.
/// </summary>
/// <param name="reader"></param>
/// <returns></returns>
private static BXDVector3 ReadBXDVector3_3_0(XmlReader reader)
{
BXDVector3 vec = new BXDVector3();
foreach (string name in IOUtilities.AllElements(reader))
{
switch (name)
{
case "X":
// Assign the x value.
vec.x = float.Parse(reader.ReadElementContentAsString());
break;
case "Y":
// Assign the y value.
vec.y = float.Parse(reader.ReadElementContentAsString());
break;
case "Z":
// Assign the z value.
vec.z = float.Parse(reader.ReadElementContentAsString());
break;
}
}
return(vec);
}
protected override void ReadBinaryJointInternal(System.IO.BinaryReader reader)
{
basePoint = reader.ReadRWObject <BXDVector3>();
axis = reader.ReadRWObject <BXDVector3>();
byte limits = reader.ReadByte();
hasAngularLimit = (limits & 1) == 1;
hasLinearStartLimit = (limits & 2) == 2;
hasLinearEndLimit = (limits & 4) == 4;
if (hasAngularLimit)
{
angularLimitLow = reader.ReadSingle();
angularLimitHigh = reader.ReadSingle();
}
if (hasLinearStartLimit)
{
linearLimitStart = reader.ReadSingle();
}
if (hasLinearEndLimit)
{
linearLimitEnd = reader.ReadSingle();
}
currentLinearPosition = reader.ReadSingle();
currentAngularPosition = reader.ReadSingle();
EnforceOrder();
}
/// <summary>
/// Subtracts the given vector from this vector and returns this object. (For method chaining)
/// </summary>
/// <param name="f">The vector to subtract</param>
/// <returns>This vector.</returns>
public BXDVector3 Subtract(BXDVector3 f)
{
x -= f.x;
y -= f.y;
z -= f.z;
return(this);
}
/// <summary>
/// Load the data from a BXDAMesh into the node
/// </summary>
/// <param name="mesh">The mesh to load from</param>
public void loadMeshes(BXDAMesh mesh)
{
this.centerOfMass = mesh.physics.centerOfMass;
baseMesh = mesh;
meshTriangleCount = 0;
foreach (BXDAMesh.BXDASubMesh sub in mesh.meshes)
{
models.Add(new VBOMesh(sub));
foreach (BXDAMesh.BXDASurface surf in sub.surfaces)
{
meshTriangleCount += surf.indicies.Length / 3;
}
}
colliderTriangleCount = 0;
foreach (BXDAMesh.BXDASubMesh sub in mesh.colliders)
{
colliders.Add(new VBOMesh(sub));
foreach (BXDAMesh.BXDASurface surf in sub.surfaces)
{
colliderTriangleCount += surf.indicies.Length / 3;
}
}
}
/// <summary>
/// Adds the given vector to this vector and returns this object. (For method chaining)
/// </summary>
/// <param name="f">The vector to add</param>
/// <returns>This vector.</returns>
public BXDVector3 Add(BXDVector3 f)
{
x += f.x;
y += f.y;
z += f.z;
return(this);
}
public static Vector ToInventorVector(BXDVector3 v)
{
if (InventorManager.Instance == null)
{
return(null);
}
return(InventorManager.Instance.TransientGeometry.CreateVector(v.x, v.y, v.z));
}
public override bool Equals(object obj)
{
if (obj is BXDVector3)
{
BXDVector3 v = (BXDVector3)obj;
return(Math.Abs(v.x - x) < EPSILON && Math.Abs(v.y - y) < EPSILON && Math.Abs(v.z - z) < EPSILON);
}
return(false);
}
/// <summary>
/// Constructs a new instance of the FieldNode class.
/// </summary>
/// <param name="nodeID"></param>
/// <param name="physicsGroupID"></param>
public FieldNode(string nodeID, string physicsGroupID = BXDFProperties.BXDF_DEFAULT_NAME)
{
NodeID = nodeID;
Position = new BXDVector3();
Rotation = new BXDQuaternion();
SubMeshID = -1;
CollisionMeshID = -1;
PropertySetID = physicsGroupID;
}
public void GetWheelInfo(out float radius, out float width, out BXDVector3 center)
{
radius = 0;
width = 0;
center = new BXDVector3();
if (GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>() != null &&
GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
float[] dists = new float[3]; //[0] = x, [1] = y, [2] = z
//Let's assume that it's aligned on some axis. If it isn't, we have other problems
foreach (VBOMesh mesh in models)
{
double[] verts = mesh.subMesh.verts;
Vector3[] vertices = new Vector3[verts.Length / 3];
for (int i = 0; i < verts.Length; i += 3)
{
vertices[i / 3] = new Vector3((float)verts[i], (float)verts[i + 1], (float)verts[i + 2]);
}
Vector3 sum = new Vector3();
foreach (Vector3 vert in vertices)
{
sum += vert;
}
center = new BXDVector3(sum.X / vertices.Length, sum.Y / vertices.Length, sum.Z / vertices.Length);
}
foreach (VBOMesh mesh in models)
{
double[] verts = mesh.subMesh.verts;
for (int i = 0; i < verts.Length; i += 3)
{
dists[0] = (float)Math.Max(dists[0], Math.Abs(center.x - verts[i]));
dists[1] = (float)Math.Max(dists[1], Math.Abs(center.y - verts[i + 1]));
dists[2] = (float)Math.Max(dists[2], Math.Abs(center.z - verts[i + 2]));
}
}
Array.Sort(dists);
width = dists[0];
radius = (dists[1] + dists[2]) / 2f;
}
Console.WriteLine(String.Format("Found center to be <{0} {1} {2}>", center.x, center.y, center.z));
Console.WriteLine(String.Format("Found radius to be {0}", radius));
Console.WriteLine(String.Format("Found width to be {0}", width));
}
public static BXDVector3 Rotate(this Matrix4 mat, BXDVector3 v)
{
float resX = (v.x * mat[0, 0]) + (v.y * mat[0, 1])
+ (v.z * mat[0, 2]);
float resY = (v.x * mat[1, 0]) + (v.y * mat[1, 1])
+ (v.z * mat[1, 2]);
float resZ = (v.x * mat[2, 0]) + (v.y * mat[2, 1])
+ (v.z * mat[2, 2]);
return(new BXDVector3((float)resX, (float)resY, (float)resZ));
}
public static BXDVector3 Multiply(this Matrix4 mat, BXDVector3 v)
{
float resX = mat[0, 3] + (v.x * mat[0, 0]) + (v.y * mat[0, 1])
+ (v.z * mat[0, 2]);
float resY = mat[1, 3] + (v.x * mat[1, 0]) + (v.y * mat[1, 1])
+ (v.z * mat[1, 2]);
float resZ = mat[2, 3] + (v.x * mat[2, 0]) + (v.y * mat[2, 1])
+ (v.z * mat[2, 2]);
return(new BXDVector3((float)resX, (float)resY, (float)resZ));
}
/// <summary>
/// Writes the BXDVector3 to an XML file with the given XmlWriter.
/// </summary>
/// <param name="vec"></param>
/// <param name="writer"></param>
private static void WriteBXDVector3(BXDVector3 vec, XmlWriter writer, string id)
{
writer.WriteStartElement("BXDVector3");
writer.WriteAttributeString("VectorID", id);
writer.WriteElementString("X", vec.x.ToString("F4"));
writer.WriteElementString("Y", vec.y.ToString("F4"));
writer.WriteElementString("Z", vec.z.ToString("F4"));
writer.WriteEndElement();
}
/// <summary>
/// Draws a cross hair at the origin
/// </summary>
/// <param name="axis">The crosshair plane is normal to this</param>
/// <param name="bias">Use this to orient the crosshair</param>
/// <param name="size">The radius of the crosshair</param>
public static void drawCrossHair(BXDVector3 axis, BXDVector3 bias, float size)
{
BXDVector3 dir1 = BXDVector3.CrossProduct(axis, bias);
BXDVector3 dir2 = BXDVector3.CrossProduct(axis, dir1);
GL.Begin(PrimitiveType.Lines);
foreach (BXDVector3 seg in new BXDVector3[] { dir1, dir2 })
{
GL.Vertex3(-seg.x * size, -seg.y * size, -seg.z * -size);
GL.Vertex3(seg.x * size, seg.y * size, seg.z * -size);
}
GL.End();
}
/// <summary>
/// Load the data from a BXDAMesh into the node
/// </summary>
/// <param name="mesh">The mesh to load from</param>
public void loadMeshes(BXDAMesh mesh)
{
this.centerOfMass = mesh.physics.centerOfMass;
baseMesh = mesh;
foreach (BXDAMesh.BXDASubMesh sub in mesh.meshes)
{
models.Add(new VBOMesh(sub));
}
foreach (BXDAMesh.BXDASubMesh sub in mesh.colliders)
{
colliders.Add(new VBOMesh(sub));
}
}
/// <summary>
/// Draws an arc at the origin, starting at bias and rotating around axis.
/// </summary>
/// <param name="axis">The axis to rotate around</param>
/// <param name="minAngle">The starting angle</param>
/// <param name="maxAngle">The ending angle</param>
/// <param name="radius">The radius of the arc</param>
/// <param name="steps">The number of steps to use</param>
public static void drawArc(BXDVector3 axis, BXDVector3 bias, float minAngle, float maxAngle, float radius, Color4 start, Color4 end, int steps = 100)
{
GL.Begin(PrimitiveType.LineStrip);
// Arcthing
Matrix4 stepMatrix = Matrix4.CreateFromAxisAngle(axis.ToTK(), -(maxAngle - minAngle) / steps);
BXDVector3 tempVec = bias.Copy().Multiply(radius / bias.Magnitude());
for (float f = 0; f < 1.0f; f += 1f / (float)steps)
{
GL.Color4(Interpolate(start, end, f));
GL.Vertex3(tempVec.x, tempVec.y, tempVec.z);
tempVec = stepMatrix.Multiply(tempVec);
}
GL.End();
}
protected override void ReadBinaryJointInternal(System.IO.BinaryReader reader)
{
basePoint = reader.ReadRWObject <BXDVector3>();
axis = reader.ReadRWObject <BXDVector3>();
hasAngularLimit = (reader.ReadByte() & 1) == 1;
if (hasAngularLimit)
{
angularLimitLow = reader.ReadSingle();
angularLimitHigh = reader.ReadSingle();
}
currentAngularPosition = reader.ReadSingle();
EnforceOrder();
}
//Reads the position of the wheel from the file.
protected override void ReadDataInternal(BinaryReader reader)
{
type = (WheelType)reader.ReadByte();
radius = reader.ReadSingle();
width = reader.ReadSingle();
center = reader.ReadRWObject <BXDVector3>();
forwardAsympSlip = reader.ReadSingle();
forwardAsympValue = reader.ReadSingle();
forwardExtremeSlip = reader.ReadSingle();
forwardExtremeValue = reader.ReadSingle();
sideAsympSlip = reader.ReadSingle();
sideAsympValue = reader.ReadSingle();
sideExtremeSlip = reader.ReadSingle();
sideExtremeValue = reader.ReadSingle();
isDriveWheel = reader.ReadBoolean();
}
public static void Load(AssemblyDocument document, out FieldProperties fieldProps, out List <PropertySet> propertySets, out Dictionary <string, List <string> > occurrencePropSets)
{
Inventor.PropertySets inventorPropertySets = document.PropertySets;
try
{
Inventor.PropertySet p = GetPropertySet(inventorPropertySets, "synthesisField");
// Field Properties
BXDVector3[] spawnpoints = JsonConvert.DeserializeObject <BXDVector3[]>(GetProperty(p, "spawnpoints", "[]"));
if (spawnpoints == null)
{
spawnpoints = new BXDVector3[0];
}
Gamepiece[] gamepieces = JsonConvert.DeserializeObject <Gamepiece[]>(GetProperty(p, "gamepieces", "[]"));
if (gamepieces == null)
{
gamepieces = new Gamepiece[0];
}
fieldProps = new FieldProperties(spawnpoints, gamepieces);
// Property Sets
propertySets = JsonConvert.DeserializeObject <List <PropertySet> >(GetProperty(p, "propertySets", "[]"), new JsonSerializerSettings {
TypeNameHandling = TypeNameHandling.Auto
});
// Occurrences
occurrencePropSets = new Dictionary <string, List <string> >();
for (int i = 0; i < propertySets.Count(); i++)
{
occurrencePropSets.Add(propertySets[i].PropertySetID,
JsonConvert.DeserializeObject <List <string> >(GetProperty(p, "propertySets." + propertySets[i].PropertySetID + ".occurrences", "[]")));
}
}
catch (Exception e)
{
throw new FailedToLoadException(e);
}
}
protected override void ReadBinaryJointInternal(System.IO.BinaryReader reader)
{
basePoint = reader.ReadRWObject <BXDVector3>();
axis = reader.ReadRWObject <BXDVector3>();
byte limitFlags = reader.ReadByte();
hasLowerLimit = (limitFlags & 1) == 1;
hasUpperLimit = (limitFlags & 2) == 2;
if (hasLowerLimit)
{
linearLimitLow = reader.ReadSingle();
}
if (hasUpperLimit)
{
linearLimitHigh = reader.ReadSingle();
}
currentLinearPosition = reader.ReadSingle();
EnforceOrder();
}
/// <summary>
/// Computes the dot product of two vectors.
/// </summary>
/// <param name="a">One vector</param>
/// <param name="b">Another vector</param>
/// <returns>(a · b)</returns>
public static float DotProduct(BXDVector3 a, BXDVector3 b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z);
}
/// <summary>
/// Converts a BXDVector3 to an Inventor.Vector.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public static Vector ToInventorVector(BXDVector3 v)
{
return(Program.INVENTOR_APPLICATION.TransientGeometry.CreateVector(v.x, v.y, v.z));
}
/// <summary>
/// Render the node's center of mass and limits of motion along the joint it is connected to (If any)
/// </summary>
public void renderDebug(bool drawAxes)
{
// Debug Settings
GL.UseProgram(0);
GL.Disable(EnableCap.Lighting);
GL.LineWidth(2f);
GL.PushMatrix();
{
GL.MultMatrix(ref myTrans);
GL.PushAttrib(AttribMask.AllAttribBits);
{
GL.PolygonMode(MaterialFace.FrontAndBack, PolygonMode.Line);
float lerp = 0;
foreach (VBOMesh mesh in colliders)
{
GL.Color4(0f, lerp, 1f - lerp, 1f);
lerp += (1f / colliders.Count);
mesh.draw();
}
}
GL.PopAttrib();
if (GetSkeletalJoint() != null && drawAxes)
{
float crosshairLength = 100;
bool hasLinearDOF = GetSkeletalJoint().GetLinearDOF().GetEnumerator().MoveNext();
#region ROTATIONAL_SPEC
foreach (AngularDOF dof in GetSkeletalJoint().GetAngularDOF())
{
BXDVector3 dirCOM = centerOfMass.Copy().Subtract(dof.basePoint);
float offset = BXDVector3.DotProduct(dirCOM, dof.rotationAxis);
BXDVector3 baseCOM = dof.basePoint.Copy();
if (BXDVector3.CrossProduct(dirCOM, dof.rotationAxis).Magnitude() < 1E-10) // COM is on the axis. (>.>) Pick randomlyish.
{
dirCOM = new BXDVector3(.123213, 123213, 0.82134); // Certain to be random.
}
dirCOM.Multiply(1f / dirCOM.Magnitude());
baseCOM.Add(dof.rotationAxis.Copy().Multiply(offset / dof.rotationAxis.Magnitude()));
BXDVector3 direction = BXDVector3.CrossProduct(dirCOM, dof.rotationAxis);
direction = BXDVector3.CrossProduct(direction, dof.rotationAxis);
if (BXDVector3.DotProduct(dirCOM, direction) < 0)
{
direction.Multiply(-1f);
}
GL.PushMatrix();
{
GL.Translate(baseCOM.x, baseCOM.y, baseCOM.z);
if (!hasLinearDOF) // Linear limits show the axis anyways, and clipping is UGLY
{
// Rotational Axis
GL.Begin(PrimitiveType.Lines);
{
GL.Color3(1f, 0f, 0f);
GL.Vertex3(-dof.rotationAxis.x * crosshairLength, -dof.rotationAxis.y * crosshairLength,
-dof.rotationAxis.z * crosshairLength);
GL.Vertex3(dof.rotationAxis.x * crosshairLength, dof.rotationAxis.y * crosshairLength,
dof.rotationAxis.z * crosshairLength);
}
GL.End();
}
// Current
GL.Begin(PrimitiveType.Lines);
{
GL.Color3(1f, 0f, 1f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(direction.x * crosshairLength, direction.y * crosshairLength, direction.z * crosshairLength);
}
GL.End();
#region ROTATIONAL_LIMITS
if (dof.hasAngularLimits())
{
// Minpos
GL.PushMatrix();
{
GL.Rotate(180.0f / 3.14f * (dof.lowerLimit - requestedRotation), dof.rotationAxis.ToTK());
GL.Begin(PrimitiveType.Lines);
{
GL.Color3(0f, 1f, 1f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(direction.x * crosshairLength, direction.y * crosshairLength, direction.z * crosshairLength);
}
GL.End();
OGLDrawing.drawArc(dof.rotationAxis, direction, dof.lowerLimit, dof.upperLimit, crosshairLength,
Color4.Cyan, Color4.Green);
}
GL.PopMatrix(); // Begin limit matrix
// Maxpos
GL.PushMatrix();
{
GL.Rotate(180.0f / 3.14f * (dof.upperLimit - requestedRotation), dof.rotationAxis.ToTK());
GL.Begin(PrimitiveType.Lines);
{
GL.Color3(0f, 1f, 0f);
GL.Vertex3(0, 0, 0);
GL.Vertex3(direction.x * crosshairLength, direction.y * crosshairLength, direction.z * crosshairLength);
}
GL.End();
}
GL.PopMatrix(); // End limit matrix
}
else
{
// Full arc!
OGLDrawing.drawArc(dof.rotationAxis, direction, 0, 6.28f, crosshairLength, Color4.Cyan, Color4.Green);
}
#endregion
}
GL.PopMatrix(); // part -> COM-basepoint
}
#endregion
#region LINEAR_SPEC
foreach (LinearDOF dof in GetSkeletalJoint().GetLinearDOF())
{
float lower = (dof.hasLowerLinearLimit() ? dof.lowerLimit : -crosshairLength) - requestedTranslation;
float upper = (dof.hasUpperLinearLimit() ? dof.upperLimit : crosshairLength) - requestedTranslation;
BXDVector3 dirCOM = centerOfMass.Copy().Subtract(dof.basePoint);
float offset = BXDVector3.DotProduct(dirCOM, dof.translationalAxis);
BXDVector3 baseCOM = dof.basePoint.Copy();
if (BXDVector3.CrossProduct(dirCOM, dof.translationalAxis).Magnitude() < 1E-10) // COM is on the axis. (>.>) Pick randomlyish.
{
dirCOM = new BXDVector3(.123213, 123213, 0.82134); // Certain to be random.
}
dirCOM.Multiply(1f / dirCOM.Magnitude());
baseCOM.Add(dof.translationalAxis.Copy().Multiply(offset / dof.translationalAxis.Magnitude()));
BXDVector3 direction = BXDVector3.CrossProduct(dirCOM, dof.translationalAxis);
direction = BXDVector3.CrossProduct(direction, dof.translationalAxis);
if (BXDVector3.DotProduct(dirCOM, direction) < 0)
{
direction.Multiply(-1f);
}
GL.PushMatrix();
{
GL.Translate(baseCOM.x, baseCOM.y, baseCOM.z);
#region LIMITS
BXDVector3 otherDirection = BXDVector3.CrossProduct(dof.translationalAxis, direction);
GL.Begin(PrimitiveType.Lines);
{
GL.Color3(0f, 1f, 0f);
GL.Vertex3(dof.translationalAxis.ToTK() * lower);
GL.Vertex3(dof.translationalAxis.ToTK() * upper);
}
GL.End();
if (dof.hasLowerLinearLimit())
{
GL.PushMatrix();
{
GL.Translate(dof.translationalAxis.ToTK() * lower);
OGLDrawing.drawCrossHair(dof.translationalAxis, direction, crosshairLength);
}
GL.PopMatrix();
}
if (dof.hasUpperLinearLimit())
{
GL.PushMatrix();
{
GL.Translate(dof.translationalAxis.ToTK() * upper);
OGLDrawing.drawCrossHair(dof.translationalAxis, direction, crosshairLength);
}
GL.PopMatrix();
}
#endregion
}
GL.PopMatrix(); // part -> COM-basepoint
}
#endregion
}
}
GL.PopMatrix(); // World -> part matrix
// Revert Debug Settings
GL.Enable(EnableCap.Lighting);
}
/// <summary>
/// Computes the cross product of two vectors. (lhs x rhs)
/// </summary>
/// <param name="lhs">The left hand element</param>
/// <param name="rhs">The right hand element</param>
/// <returns>(lhs x rhs)</returns>
public static BXDVector3 CrossProduct(BXDVector3 lhs, BXDVector3 rhs)
{
return(new BXDVector3(lhs.y * rhs.z - lhs.z * rhs.y, lhs.z * rhs.x - lhs.x * rhs.z, lhs.x * rhs.y - lhs.y * rhs.x));
}
protected override void ReadBinaryJointInternal(System.IO.BinaryReader reader)
{
normal = reader.ReadRWObject <BXDVector3>();
basePoint = reader.ReadRWObject <BXDVector3>();
}
/// <summary>
/// Initializes a new instance of the BoxCollider class.
/// </summary>
public BoxCollider(BXDVector3 scale)
: base(PropertySetCollider.PropertySetCollisionType.BOX)
{
Scale = scale;
}
public static Vector3 AsV3(this BXDVector3 v)
{
return(new Vector3(v.x * -0.01f, v.y * 0.01f, v.z * 0.01f));
}
/// <summary>
/// Compute the positions and rotations of this node and its children
/// </summary>
/// <param name="moveJoints">Whether or not to move the node on its joints</param>
public void compute(bool moveJoints)
{
if (moveJoints)
{
timeStep += 0.005f;
}
else
{
timeStep = 0.0f;
}
#region INIT_POSITION
if (!initialPositions && GetSkeletalJoint() != null)
{
initialPositions = true;
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cjb = (CylindricalJoint_Base)GetSkeletalJoint();
requestedRotation = cjb.currentAngularPosition;
requestedTranslation = cjb.currentLinearPosition;
break;
case SkeletalJointType.ROTATIONAL:
RotationalJoint_Base rjb = (RotationalJoint_Base)GetSkeletalJoint();
requestedRotation = rjb.currentAngularPosition;
requestedTranslation = 0;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base ljb = (LinearJoint_Base)GetSkeletalJoint();
requestedRotation = 0;
requestedTranslation = ljb.currentLinearPosition;
break;
}
}
#endregion
myTrans = Matrix4.Identity;
if (GetSkeletalJoint() != null)
{
foreach (AngularDOF dof in GetSkeletalJoint().GetAngularDOF())
{
BXDVector3 axis = dof.rotationAxis;
BXDVector3 basePoint = dof.basePoint;
if (GetParent() != null)
{
basePoint = ((OGL_RigidNode)GetParent()).myTrans.Multiply(basePoint);
axis = ((OGL_RigidNode)GetParent()).myTrans.Rotate(axis);
}
requestedRotation = (float)(Math.Sin(timeStep) + 0.9f) * 1.2f *
(dof.hasAngularLimits() ?
(dof.upperLimit - dof.lowerLimit) / 2.0f : 3.14f) +
(dof.hasAngularLimits() ?
dof.lowerLimit : 0);
requestedRotation = Math.Max(dof.lowerLimit, Math.Min(dof.upperLimit, requestedRotation));
myTrans *= Matrix4.CreateTranslation(-dof.basePoint.ToTK());
myTrans *= Matrix4.CreateFromAxisAngle(dof.rotationAxis.ToTK(), requestedRotation - dof.currentPosition);
myTrans *= Matrix4.CreateTranslation(dof.basePoint.ToTK());
}
foreach (LinearDOF dof in GetSkeletalJoint().GetLinearDOF())
{
requestedTranslation = (float)(Math.Cos(timeStep) + 0.9f) * 1.2f *
(dof.hasLowerLinearLimit() && dof.hasUpperLinearLimit() ?
(dof.upperLimit - dof.lowerLimit) / 2.0f : 3.14f) +
(dof.hasLowerLinearLimit() ?
dof.lowerLimit : 0);
requestedTranslation = Math.Max(dof.lowerLimit, Math.Min(dof.upperLimit, requestedTranslation));
myTrans *= Matrix4.CreateTranslation(dof.translationalAxis.ToTK() * (requestedTranslation - dof.currentPosition));
}
}
if (GetParent() != null)
{
myTrans = myTrans * ((OGL_RigidNode)GetParent()).myTrans;
}
foreach (KeyValuePair <SkeletalJoint_Base, RigidNode_Base> pair in Children)
{
OGL_RigidNode child = ((OGL_RigidNode)pair.Value);
child.compute(moveJoints);
}
}
public static Vector3 ToTK(this BXDVector3 vec)
{
return(new Vector3(vec.x, vec.y, vec.z));
}
public WheelDriverMeta()
{
center = new BXDVector3();
}
