private bool IsSupportConeWithinXYAngleRange(SupportConeV2 supportCone1, SupportConeV2 supportCone2, float angleMin, float angleMax) { var xyAngleBetweenSupportCones = VectorHelper.CalcAngleBetweenVectorPoints(supportCone1.RefPointCSegment.Xy, this.RefPointCSegment.Xy, supportCone2.RefPointCSegment.Xy); Console.WriteLine(xyAngleBetweenSupportCones); if (xyAngleBetweenSupportCones < angleMin || xyAngleBetweenSupportCones > angleMax) { return(true); } else { return(false); } }
private void Update(Vector3Class startPoint, Vector3Class endPoint) { this.StartPoint = startPoint; this.EndPoint = endPoint; float yAngle = 0f; float zAngle = 0f; VectorHelper.CalcRotationAnglesYZFromVector(endPoint - startPoint, false, out zAngle, out yAngle); var diffVector = endPoint - startPoint; var diffVector2 = diffVector; diffVector2.Z = 0; yAngle = VectorHelper.CalcAngleBetweenVectorPoints(new Vector2(diffVector2.Length, 0), new Vector2(0, 0), new Vector2(diffVector2.Length, diffVector.Z)); var totalHeight = (endPoint - startPoint).Length; if (this.Triangles == null) { this.Triangles = new TriangleInfoList(); } this.Triangles[0].Clear(); this.Triangles = new TriangleInfoList(); var capPointsCount = 26; var capPoints = VectorHelper.CreateCircle(0, this.Radius, capPointsCount, true); var rotationMatrix = Matrix4.CreateRotationY(OpenTK.MathHelper.DegreesToRadians(yAngle)); for (var capPointIndex = 0; capPointIndex < capPointsCount; capPointIndex++) { var rotatedVector = Vector3Class.Transform(capPoints[capPointIndex], rotationMatrix); var rotationXYScaling = capPoints[capPointIndex].X / rotatedVector.X; capPoints[capPointIndex] = new Vector3Class(capPoints[capPointIndex].X, capPoints[capPointIndex].Y, rotatedVector.Z * rotationXYScaling); } for (var capPointIndex = 0; capPointIndex < capPointsCount; capPointIndex++) { var capTriangle = new Triangle(); capTriangle.Vectors[0].Position = capPoints[capPointIndex]; capTriangle.Vectors[1].Position = capPoints[capPointIndex == capPointsCount - 1 ? 0 : capPointIndex + 1]; //capTriangle.Vectors[2].Position = startPoint; capTriangle.CalcNormal(); this.Triangles[0].Add(capTriangle); } //bottom capPoints for (var capPointIndex = 0; capPointIndex < capPointsCount; capPointIndex++) { var bottomCapTriangle = (Triangle)this.Triangles[0][capPointIndex].Clone(); bottomCapTriangle.Flip(); this.Triangles[0].Add(bottomCapTriangle); } //move top cap to total height for (var capPointIndex = 0; capPointIndex < capPointsCount; capPointIndex++) { this.Triangles[0][capPointIndex].Vectors[0].Position.Z += totalHeight; this.Triangles[0][capPointIndex].Vectors[1].Position.Z += totalHeight; this.Triangles[0][capPointIndex].Vectors[2].Position.Z += totalHeight; } //create cylinder triangles for (var capPointIndex = 0; capPointIndex < capPointsCount; capPointIndex++) { var leftTriangle = new Triangle(); leftTriangle.Vectors[0].Position = this.Triangles[0][capPointIndex].Vectors[0].Position; leftTriangle.Vectors[1].Position = this.Triangles[0][capPointIndex + capPointsCount].Vectors[0].Position; leftTriangle.Vectors[2].Position = this.Triangles[0][capPointIndex + capPointsCount].Vectors[2].Position; leftTriangle.CalcNormal(); this.Triangles[0].Add(leftTriangle); var rightTriangle = new Triangle(); rightTriangle.Vectors[0].Position = this.Triangles[0][capPointIndex].Vectors[0].Position; rightTriangle.Vectors[1].Position = this.Triangles[0][capPointIndex + capPointsCount].Vectors[2].Position; rightTriangle.Vectors[2].Position = this.Triangles[0][capPointIndex].Vectors[1].Position; rightTriangle.CalcNormal(); this.Triangles[0].Add(rightTriangle); } this.UpdateBoundries(); this.Rotate(0, 90 - yAngle, 0, Events.RotationEventArgs.TypeAxis.Y, updateFaceColor: false); this.Rotate(0, 0, zAngle, Events.RotationEventArgs.TypeAxis.Z, updateFaceColor: false); foreach (var triangle in this.Triangles[0]) { triangle.Vectors[0].Position += startPoint; triangle.Vectors[1].Position += startPoint; triangle.Vectors[2].Position += startPoint; } this.UpdateBoundries(); }