public AddJoinedBodiesArgs(AddJointType jointType, AddJointBodyType body1Type, AddJointBodyType body2Type, double separationDistance) { this.JointType = jointType; this.Body1Type = body1Type; this.Body2Type = body2Type; this.SeparationDistance = separationDistance; }
private void GetJointBodyPair(out Body body1, out Body body2, AddJointBodyType bodyType1, AddJointBodyType bodyType2, Point3D centerPoint, Quaternion rotation, double separationDistance, Color color) { #region Body 1 double distanceToPermiter; Quaternion localRotation; GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType1); Vector3D offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0); // adding to the separation distance because I don't want centerpoint to centerpoint, I want edge to edge offset = rotation.GetRotatedVector(offset); Point3D shiftedCenter = centerPoint + offset; localRotation = rotation.ToUnit() * localRotation.ToUnit(); RotateTransform3D finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation)); body1 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType1), shiftedCenter, finalRotation, color); #endregion #region Body 2 GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType2); offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0); offset = rotation.GetRotatedVector(offset); shiftedCenter = centerPoint - offset; // subtracting instead of adding localRotation = new Quaternion(new Vector3D(0, 0, 1), 180d).ToUnit() * rotation.ToUnit() * localRotation.ToUnit(); // throwing in an extra 180 degrees of spin finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation)); body2 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType2), shiftedCenter, finalRotation, color); #endregion }
private static CollisionShapeType GetJointBodyPairSprtHullType(AddJointBodyType bodyType) { switch (bodyType) { case AddJointBodyType.Box_Corner: case AddJointBodyType.Box_Edge: case AddJointBodyType.Box_Face: return(CollisionShapeType.Box); case AddJointBodyType.Cone_Base: case AddJointBodyType.Cone_Tip: return(CollisionShapeType.Cone); case AddJointBodyType.Cylinder_Cap: case AddJointBodyType.Cylinder_Edge: return(CollisionShapeType.Cylinder); case AddJointBodyType.Sphere: return(CollisionShapeType.Sphere); default: throw new ApplicationException("Unknown AddJointBodyType: " + bodyType.ToString()); } }
private static CollisionShapeType GetJointBodyPairSprtHullType(AddJointBodyType bodyType) { switch (bodyType) { case AddJointBodyType.Box_Corner: case AddJointBodyType.Box_Edge: case AddJointBodyType.Box_Face: return CollisionShapeType.Box; case AddJointBodyType.Cone_Base: case AddJointBodyType.Cone_Tip: return CollisionShapeType.Cone; case AddJointBodyType.Cylinder_Cap: case AddJointBodyType.Cylinder_Edge: return CollisionShapeType.Cylinder; case AddJointBodyType.Sphere: return CollisionShapeType.Sphere; default: throw new ApplicationException("Unknown AddJointBodyType: " + bodyType.ToString()); } }
private static void GetJointBodyPairSprtOffset(out double distance, out Quaternion rotation, AddJointBodyType bodyType) { switch (bodyType) { case AddJointBodyType.Box_Corner: distance = Math.Sqrt(3d); //TODO: Figure out how to rotate a cube onto its corner rotation = new Quaternion(new Vector3D(0, 0, 1), 45d).ToUnit() * new Quaternion(new Vector3D(0, 1, 0), 45d).ToUnit(); //rotation = new Quaternion(new Vector3D(0, 0, 1), 45d).ToUnit() + new Quaternion(new Vector3D(0, 1, 0), 45d).ToUnit() + new Quaternion(new Vector3D(1, 0, 0), 45d).ToUnit(); break; case AddJointBodyType.Box_Edge: distance = Math.Sqrt(2d); rotation = new Quaternion(new Vector3D(0, 0, 1), 45d); break; case AddJointBodyType.Cone_Tip: //TODO: Finish these distance = 1d; rotation = new Quaternion(new Vector3D(0, 0, 1), 180d); break; case AddJointBodyType.Cylinder_Edge: distance = 1d; rotation = new Quaternion(new Vector3D(0, 0, 1), 90d); break; case AddJointBodyType.Box_Face: case AddJointBodyType.Cone_Base: case AddJointBodyType.Cylinder_Cap: case AddJointBodyType.Sphere: distance = 1d; rotation = new Quaternion(new Vector3D(0, 0, 1), 0d); break; default: throw new ApplicationException("Unknown AddJointBodyType: " + bodyType.ToString()); } }