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