private ICollisionJoint[] GetCarConstraints(List <ICollisionShape> shape)
{
ICollisionJoint[] constraints = new ICollisionJoint[4];
//Wheels Joints
constraints[0] = new Hinge2Joint(
shape[1],
shape[2],
shape[3],
new Vector3d(-1.1, -0.5, -1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[0].SetAxis1AngularLimit(0.0, 0.0);
constraints[1] = new Hinge2Joint(
shape[1],
shape[4],
shape[5],
new Vector3d(1.1, -0.5, 1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[1].SetAxis1AngularLimit(-0.78539816339, 0.78539816339);
constraints[2] = new Hinge2Joint(
shape[1],
shape[3],
shape[2],
new Vector3d(1.1, -0.5, -1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[2].SetAxis1AngularLimit(0.0, 0.0);
constraints[3] = new Hinge2Joint(
shape[1],
shape[5],
shape[4],
new Vector3d(-1.1, -0.5, 1.5),
new Vector3d(0.0, 1.0, 0.0),
new Vector3d(1.0, 0.0, 0.0),
0.8,
0.01,
0.01);
constraints[3].SetAxis1AngularLimit(-0.78539816339, 0.78539816339);
return(constraints);
}
private ICollisionJoint[] GetBridgeConstraints(List <ICollisionShape> shape)
{
ICollisionJoint[] constraints = new ICollisionJoint[30];
int idx = 0;
double zValue = -1.8;
double yValue = 1.2;
for (int i = 0; i < 10; i++)
{
constraints[idx] = new BallAndSocketJoint(
shape[i],
shape[i + 1],
new Vector3d(0.5, yValue, zValue),
0.9,
0.00016);
idx++;
constraints[idx] = new BallAndSocketJoint(
shape[i],
shape[i + 1],
new Vector3d(-0.5, yValue, zValue),
0.9,
0.00016);
idx++;
constraints[idx] = new AngularJoint(
shape[i],
shape[i + 1],
new Vector3d(-0.5, yValue, zValue),
new Vector3d(1.0, 0.0, 0.0),
new Vector3d(0.0, 1.0, 0.0),
0.5,
0.008,
0.008);
idx++;
zValue = -1.25;
yValue = 0.0;
}
return(constraints);
}
private ICollisionJoint[] getConstraint(
ICollisionShape[] shape)
{
ICollisionJoint[] constraints = new ICollisionJoint[2];
constraints[0] = new FixedJoint(
shape[1],
shape[2],
60.0,
0.0);
constraints[1] = new FixedJoint(
shape[2],
shape[3],
60.0,
0.0);
return(constraints);
}
//public List<Tuple<Vector3d, Vector3d>> GetShapesAABB()
//{
// var result = new List<Tuple<Vector3d, Vector3d>>();
// for (int i = 0; i < Shapes.Length; i++)
// {
// if (Shapes[i] is ShapeDefinition.ConvexShape)
// result.Add(new Tuple<Vector3d, Vector3d>(Shapes[i].AABBox.Min, Shapes[i].AABBox.Max));
// else if (Shapes[i] is ShapeDefinition.ConcaveShape)
// {
// var shapesGeom = ((ShapeDefinition.ConcaveShape)Shapes[i]).ConvexShapesGeometry;
// for (int j = 0; j < shapesGeom.Length; j++)
// {
// result.Add(new Tuple<Vector3d, Vector3d>(shapesGeom[j].AABBox.Min, shapesGeom[j].AABBox.Max));
// }
// result.Add(new Tuple<Vector3d, Vector3d>(Shapes[i].AABBox.Min, Shapes[i].AABBox.Max));
// }
// }
// return result;
//}
//TODO Test Hierarchical tree intersection
//public List<Tuple<Vector3d, Vector3d>> GetHierarchicalIntersection()
//{
// var result = new List<Tuple<Vector3d, Vector3d>>();
// int height = HierarchicalTree.GetMaxHeight();
// for (int i = 0; i < Shapes.Length; i++)
// {
// var overlaps = HierarchicalTree.QueryOverlaps(ExtractIAABBFromShape(Shapes[i]));
// foreach (var item in overlaps)
// {
// var aabb = item.GetAABB();
// result.Add(new Tuple<Vector3d, Vector3d>(aabb.Min, aabb.Max));
// }
// }
// return result;
//}
#endregion
#region Simulation Joint
public void AddJoint(ICollisionJoint joint)
{
CollisionJoints.Add(joint);
var mappedJoint = ((IMapperJoint)joint).GetJoint();
if (Joints != null &&
Joints.Count > 0)
{
Joints.Add(mappedJoint);
}
else
{
Joints = new List <IConstraint>
{
mappedJoint
};
}
}
public ICollisionJoint[] LoadSimulationJoints(
ICollisionShape[] objects)
{
var xmlDoc = new XmlDocument();
xmlDoc.Load(FileNameObjectProperties);
XmlNodeList xmlList = xmlDoc.SelectNodes(nodePathJoints);
ICollisionJoint[] joints = new ICollisionJoint[xmlList.Count];
for (int i = 0; i < xmlList.Count; i++)
{
//Object index A
int indexA = Convert.ToInt32(xmlList[i][objectIndexAAttribute].InnerText);
//Object index B
int indexB = Convert.ToInt32(xmlList[i][objectIndexBAttribute].InnerText);
XmlNodeList jointPropertiesList = xmlList[i].SelectNodes(jointProperties);
ICollisionJoint[] joint = new ICollisionJoint[jointPropertiesList.Count];
for (int j = 0; j < jointPropertiesList.Count; j++)
{
//Joint type
var jointType = (JointType)Convert.ToInt32(jointPropertiesList[j][this.jointType].InnerText);
//Restore coefficient
double K = Convert.ToDouble(jointPropertiesList[j][restoreCoeffAttribute].InnerText);
//Stretch coefficient
double C = Convert.ToDouble(jointPropertiesList[j][stretchCoeffAttribute].InnerText);
//Position
var startAnchorPosition = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][positionJointAttribute].Attributes["z"].Value));
//Action Axis
var actionAxis = new Vector3d(
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["x"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["y"].Value),
Convert.ToDouble(jointPropertiesList[j][this.actionAxis].Attributes["z"].Value));
switch (jointType)
{
case JointType.Fixed:
joint [j] = new FixedJoint(
objects[indexA],
objects[indexB],
K,
C);
break;
case JointType.BallAndSocket:
joint[j] = new BallAndSocketJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
K,
C);
break;
case JointType.Slider:
joint[j] = new SliderJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetLinearLimit(Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText), Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Piston:
joint[j] = new PistonJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetLinearLimit(
Convert.ToDouble(jointPropertiesList[j][linearLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][linearLimitMax].InnerText));
break;
case JointType.Hinge:
joint[j] = new HingeJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis1Motor(3.0, 0.15);
break;
case JointType.Universal:
joint[j] = new UniversalJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
joint[j].SetAxis2AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
break;
case JointType.Hinge2:
joint[j] = new Hinge2Joint(
objects[indexA],
objects[indexB],
startAnchorPosition,
actionAxis,
new Vector3d(1.0, 0.0, 0.0),
K,
1.0,
C);
joint[j].SetAxis1AngularLimit(
Convert.ToDouble(jointPropertiesList[j][angularLimitMin].InnerText),
Convert.ToDouble(jointPropertiesList[j][angularLimitMax].InnerText));
//joint[j].SetAxis2Motor(4.0, 3.0);
break;
case JointType.Angular:
joint[j] = new AngularJoint(
objects[indexA],
objects[indexB],
startAnchorPosition,
new Vector3d(1.0, 0.0, 0.0),
new Vector3d(0.0, 1.0, 0.0),
0.16,
0.008,
0.008);
break;
}
joints[i] = joint[j];
}
}
return(joints);
}
