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