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