/// <summary>
/// 求指定点与指定射线方向的线段与球的交点，假定球心始终为0,0,0
/// </summary>
/// <param name="cp">线的点</param>
/// <param name="dir">射线方向</param>
/// <returns></returns>
public static System.Windows.Media.Media3D.Point3D? getCrossBall(System.Windows.Media.Media3D.Point3D cp, System.Windows.Media.Media3D.Vector3D dir, double rad)
{
dir.Normalize();
//B=2(xd(x0-xc)+yd(y0-yc)+zd(z0-zc))
//C=(x0-xc)^2+(y0-yc)^2+(z0-zc)^2-sr^2
//B=2*(xd*x0+yd*y0+zd*z0)
//C=x0^2+y0^2+z0^2-sr^2
double B = 2 * (dir.X * cp.X + dir.Y * cp.Y + dir.Z * cp.Z);
double C = cp.X * cp.X + cp.Y * cp.Y + cp.Z * cp.Z - rad * rad;
double root = B * B - 4 * C;
if (root > 0)
{
double t0 = (-B - Math.Pow(root, 0.5)) / 2;
double t1 = (-B + Math.Pow(root, 0.5)) / 2;
double t;
if (t0 > 0 && t1 > 0)
{
t = Math.Min(t0, t1);
}
else if (t0 > 0 || t1 > 0)
{
t = Math.Max(t0, t1);
}
else
{
return(null); //射线方向反向
}
return(new System.Windows.Media.Media3D.Point3D(cp.X + t * dir.X, cp.Y + t * dir.Y, cp.Z + t * dir.Z));
}
return(null);
}
private static async Task <MapPoint> CreatePointAlongArc(MapPoint startPt, MapPoint endPt, MapPoint centerPt, double angle, double radius, SpatialReference spatRef, bool arcIsMinor, bool arcIsCounterClockwise)
{
System.Windows.Media.Media3D.Vector3D start = new System.Windows.Media.Media3D.Vector3D(startPt.X - centerPt.X, startPt.Y - centerPt.Y, startPt.Z - centerPt.Z);
System.Windows.Media.Media3D.Vector3D end = new System.Windows.Media.Media3D.Vector3D(endPt.X - centerPt.X, endPt.Y - centerPt.Y, endPt.Z - centerPt.Z);
System.Windows.Media.Media3D.Vector3D normalOfPlane = new System.Windows.Media.Media3D.Vector3D();
normalOfPlane = System.Windows.Media.Media3D.Vector3D.CrossProduct(start, end);
//Two ortho vectors: orthoVec and start
System.Windows.Media.Media3D.Vector3D orthoVec = new System.Windows.Media.Media3D.Vector3D();
orthoVec = System.Windows.Media.Media3D.Vector3D.CrossProduct(normalOfPlane, start);
//If this is not done then half of the keyframes for S-shaped curve are not on the curve
if (arcIsMinor && !arcIsCounterClockwise)
{
orthoVec.Negate();
}
//Normalize
start.Normalize();
orthoVec.Normalize();
System.Windows.Media.Media3D.Vector3D ptAlong = new System.Windows.Media.Media3D.Vector3D();
ptAlong = radius * Math.Cos(angle) * start + radius * Math.Sin(angle) * orthoVec;
MapPoint intermediateKeyframePoint = await QueuedTask.Run(() => MapPointBuilder.CreateMapPoint(ptAlong.X + centerPt.X, ptAlong.Y + centerPt.Y, ptAlong.Z + centerPt.Z, spatRef));
return(intermediateKeyframePoint);
}
private static async Task <MapPoint> CreatePointAlongSegment(MapPoint startPt, MapPoint endPt, double distanceFromStartPoint, SpatialReference spatRef)
{
System.Windows.Media.Media3D.Point3D fromPt = new System.Windows.Media.Media3D.Point3D(startPt.X, startPt.Y, startPt.Z);
System.Windows.Media.Media3D.Point3D toPt = new System.Windows.Media.Media3D.Point3D(endPt.X, endPt.Y, endPt.Z);
System.Windows.Media.Media3D.Vector3D lineVec = new System.Windows.Media.Media3D.Vector3D(toPt.X - fromPt.X, toPt.Y - fromPt.Y, toPt.Z - fromPt.Z);
lineVec.Normalize();
System.Windows.Media.Media3D.Point3D ptAlong = new System.Windows.Media.Media3D.Point3D(fromPt.X + distanceFromStartPoint * (lineVec.X),
fromPt.Y + distanceFromStartPoint * (lineVec.Y),
fromPt.Z + distanceFromStartPoint * (lineVec.Z));
MapPoint intermediateKeyframePoint = await QueuedTask.Run(() => MapPointBuilder.CreateMapPoint(ptAlong.X, ptAlong.Y, ptAlong.Z, spatRef));
return(intermediateKeyframePoint);
}
public MainViewModel()
{
EffectsManager = new DefaultEffectsManager();
// camera setup
Camera = new PerspectiveCamera
{
Position = new Point3D(0, -2000, 400),
LookDirection = new Vector3D(0, 2000, 0),
UpDirection = UpDirection,
FarPlaneDistance = 5000000,
};
// setup lighting
//SkyboxTexture = LoadFileToMemory("Cubemap_Grandcanyon.dds");
LightDirection = new Vector3D(0, -0.5, -1);
LightDirection.Normalize();
SetHeadLight();
AmbientLightColor = Colors.White * 1000;
DirectionalLightColor = Color.FromScRgb(1, 10, 10, 10);
BackgroundColor = Color.FromRgb(40, 40, 40);
// model transform
ModelTransform = Transform3D.Identity;
//var r = new System.Windows.Media.Media3D.AxisAngleRotation3D(new Vector3D(0, 0, 1), 90);
//EnvironmentTransform = new System.Windows.Media.Media3D.RotateTransform3D(r);
// model materials
var material = new PBRMaterial();
material.AlbedoColor = new Color4(0.8f, 0.4f, 0.05f, 1f);
material.MetallicFactor = 0.2;
material.ReflectanceFactor = 0.8;
material.RenderEnvironmentMap = false;
material.AmbientOcclusionFactor = 1;
// floor plane grid
Grid = LineBuilder.GenerateGrid(new Vector3(0, 0, 1), -10, 10, -10, 10);
GridColor = Color.FromRgb(100, 100, 100);
GridTransform = new System.Windows.Media.Media3D.ScaleTransform3D(100, 100, 100);
// commands
UpZCommand = new RelayCommand(_ => { Log = "test switch"; });
// robot program
var test = new TestProgram();
Program = test.Program;
Log = Program.Errors.Count == 0 ? Program.RobotSystem.Name : string.Join(" ", Program.Errors);
// make robot
var cell = Program.RobotSystem as Robots.RobotCell;
var group = cell.MechanicalGroups.First();
var meshes = group.Joints.Select(j => j.Mesh).Prepend(group.Robot.BaseMesh);
_origins = group.Joints.Select(j => j.Plane).Prepend(group.Robot.BasePlane).ToArray();
foreach (var joint in meshes)
{
var model = new MeshGeometryModel3D();
model.Geometry = joint.ToWPF();
model.Material = material;
model.Transform = Transform3D.Identity;
model.IsThrowingShadow = true;
RobotModels.Add(model);
}
// timer
_timer = new Timer(Tick, null, Timeout.Infinite, 0);
}
public System.Windows.Media.Media3D.Vector3D CalculateMaxInertialTorqueWithoutMass()
{
inertialTorque =System.Windows.Media.Media3D.Vector3D.CrossProduct(displacement, armCM);
if (inertialTorque.Length!=0)
inertialTorque.Normalize();
inertialTorque.X *= (UPPER_ARM_INERTIA_RATE+FORE_ARM_INERTIA_RATE)*angularAcc;
inertialTorque.Y *= (UPPER_ARM_INERTIA_RATE + FORE_ARM_INERTIA_RATE) * angularAcc;
inertialTorque.Z *= (UPPER_ARM_INERTIA_RATE + FORE_ARM_INERTIA_RATE) * angularAcc;
inertialTorqueWithoutMass.X = inertialTorque.X / armMass;
inertialTorqueWithoutMass.Y = inertialTorque.Y / armMass;
inertialTorqueWithoutMass.Z = inertialTorque.Z / armMass;
return inertialTorqueWithoutMass;
}
public VECTOR3D normalize()
{
System.Windows.Media.Media3D.Vector3D v = new System.Windows.Media.Media3D.Vector3D(x, y, z);
v.Normalize();
return(new VECTOR3D((float)v.X, (float)v.Y, (float)v.Z));
}
