public static Matrix <double> GenerateRotate3D(double angleX, double angleY, double angleZ = 0, AngleMode angleMode = AngleMode.Radians, double centerX = 0, double centerY = 0, double centerZ = 0)
{
var translation = GenerateTranslate3D(centerX, centerY, centerZ);
angleX = angleMode == AngleMode.Radians ? angleX : AngleConverter.ConvertDegreesToRadians(angleX);
angleY = angleMode == AngleMode.Radians ? angleY : AngleConverter.ConvertDegreesToRadians(angleY);
angleZ = angleMode == AngleMode.Radians ? angleZ : AngleConverter.ConvertDegreesToRadians(angleZ);
// TODO: Calculate short
var rotationX = new Matrix <double>(3, 3);
rotationX[0, 0] = 1;
rotationX[0, 1] = 0;
rotationX[0, 2] = 0;
rotationX[1, 0] = 0;
rotationX[1, 1] = Math.Cos(angleX);
rotationX[1, 2] = -Math.Sin(angleX);
//rotationX[];
Matrix <double> rotationY = new Matrix <double>(3, 3);
Matrix <double> rotationZ = new Matrix <double>(3, 3);
return(new Matrix <double>()); // temp
}
private static void RunOptions(Options opts)
{
// TODO: Make sure the best versions of these generic collections are being used in the right places.
// Use arrays when size is known and is not expected to change
// Otherwise, use a list
// Consider places where we can avoid a conversion of .ToArray/.ToList and rely on the IEnumerable interface
try
{
Mesh3 model = new Mesh3(ReadFacetsFromFile(opts.StlInputPath, opts.IsStlAscii));
Console.WriteLine("Read " + model.Facets.Length + " facets from file");
Polygon3[] unsupportedFacets = model.Facets.Where(facet => DoesFacetNeedSupported(facet, opts.CriticalAngle)).ToArray();
Console.WriteLine("Identified " + unsupportedFacets.Length + " unsupported facets");
Point3Tree <List <Polygon3> > edgeFacetIndex = new Point3Tree <List <Polygon3> >(GetEdgeFacetKeys(unsupportedFacets));
Console.WriteLine("Created an index with " + edgeFacetIndex.Keys.Length + " edges");
CreateEdgeFacetAssociation(unsupportedFacets, edgeFacetIndex);
Console.WriteLine("Association created between facets and edges");
List <Mesh3> unsupportedRegions = BuildUnsupportedRegions(unsupportedFacets, edgeFacetIndex);
Console.WriteLine("Built " + unsupportedRegions.Count + " unsupported regions");
List <Mesh3> largeRegions = unsupportedRegions.Where(region => IsLargeRegion(region, edgeFacetIndex, opts.DimensionLength, opts.ToleranceAngle)).ToList();
Console.WriteLine("Removed " + (unsupportedRegions.Count - largeRegions.Count) + " small unsupported regions");
List <Polygon3> scaffoldingFacets = new List <Polygon3>();
if (opts.DoXScaffolding || opts.DoYScaffolding)
{
List <Vector3> supportNormals = new List <Vector3>();
Quaternion rotation = new Quaternion(0, 0, (float)AngleConverter.DegToRad(opts.ScaffoldingAngle));
if (opts.DoXScaffolding)
{
supportNormals.Add(Vector3.Transform(YZNormal, rotation));
}
if (opts.DoYScaffolding)
{
supportNormals.Add(Vector3.Transform(XZNormal, rotation));
}
Console.WriteLine("Made support normals");
foreach (Vector3 supportNormal in supportNormals)
{
scaffoldingFacets.AddRange(GenerateLineScaffolding(model, largeRegions, supportNormal, (float)opts.SupportSpacing, (float)opts.PlateSpacing));
}
}
if (opts.DoContourScaffolding)
{
scaffoldingFacets.AddRange(GenerateContourScaffolding(largeRegions, (float)opts.PlateSpacing, edgeFacetIndex));
}
StlBinaryWriter writer = new StlBinaryWriter();
writer.Write(opts.StlOutputPath, scaffoldingFacets.ToArray());
}
catch (Exception ex)
{
Console.Error.WriteLine(ex);
Environment.Exit(1);
}
}
void Start()
{
pivot = GameObject.Find("pivot").GetComponent <Transform>();
angleConv = new AngleConverter();
_acceleration = Vector3.zero;
_velocity = Vector3.zero;
_velocityFactor = 0f;
player = GameObject.FindGameObjectWithTag("Player").GetComponent <Transform>();
}
public void LoadCalculatorAsset()
{
InputBuffer = new InputBuffer();
Operators = new OperatorLookup();
UnitConverter = new AngleConverter();
OperatorConverter = new OperatorConverter(Operators.Operators, Operators.Unary);
Parenthesizer = new Parenthesizer(Operators.Precedence);
ExpressionBuilder = new ExpressionBuilder(Parenthesizer);
ExpressionParser = new ExpressionParser(OperatorConverter);
Evaluator = new Evaluator(UnitConverter, OperatorConverter, Operators);
MemoryStorage = new MemoryStorage();
}
private void updateJointState(JointState jointState)
{
if (jointState_.name == null)
{
return;
}
for (int i = 0; i < jointState.name.Count; i++)
{
jointsGrid[2, i].Value =
AngleConverter.RadianToDegrees(jointState.position[i]).ToString("F2");
}
}
protected override void Calculate()
{
if (textBox_FromUnit.Text != string.Empty)
{
AngleConverter angleConverter = new AngleConverter(decimal.Parse(textBox_FromUnit.Text), comboBox_FromUnit.Text, comboBox_ToUnit.Text);
textBox_ToUnit.Text = angleConverter.ConvertUnit().ToString("#.#####");
}
else
{
return;
}
}
private static bool IsLargeRegion(Mesh3 region, Point3Tree <List <Polygon3> > edgeFacetIndex, double dimensionLength, double toleranceAngle)
{
bool isLargeRegion = false;
List <Vector3> largeDiagonals = GetLargeDiagonals(GetBoundingVertices(region, edgeFacetIndex), dimensionLength);
for (int i = 0; i != largeDiagonals.Count && !isLargeRegion; i++)
{
Vector3 diagonal1 = largeDiagonals[i];
for (int j = i; j != largeDiagonals.Count && !isLargeRegion; j++)
{
Vector3 diagonal2 = largeDiagonals[j];
double angleBetween = AngleConverter.RadToDeg(Vector3.CalculateAngle(diagonal1, diagonal2));
if (angleBetween >= 90 - toleranceAngle && angleBetween <= 90 + toleranceAngle)
{
isLargeRegion = true;
}
}
}
return(isLargeRegion);
}
// 실질적인 발키리 기체 제어 함수
void Thread1_UpdateObject(float rotation, float leftSpeed, float rightSpeed, float sideSpeed)
{
const int addToDelay = 1;
// SET SPEED
float speed = (leftSpeed + rightSpeed) * 7.0f;
if (Mathf.Abs(leftSpeed) == Mathf.Abs(rightSpeed))
{
speed *= 2.0f;
}
/// ANIMATION -> ROLL, PITCH
{
//움직임 관련, 앞으로 갈때 기우뚱
Vector2 angle = Thread1_WalkingAnimation(leftSpeed, rightSpeed, sideSpeed);
renderedRoll = AngleConverter.LerpAngle(renderedRoll, angle.x, 0.1f);
renderedPitch = AngleConverter.LerpAngle(renderedPitch, angle.y, 0.1f);
// WRITE
basic[0].Roll = AngleConverter.ToDeviceVer1(renderedRoll); // X
basic[0].Pitch = AngleConverter.ToDeviceVer1(renderedPitch); // Y
basic[0].Heave = AngleConverter.ToDeviceVer1(bodyAngle.sy * 100.0f); // 높낮이 수정
basic[0].Delay = 1;
}
/// YAW (Z)
{
// READ
double current = AngleConverter.ToTypicalVer2(output.Yaw);
renderedYaw = AngleConverter.LerpAngle(renderedYaw, (float)current, 0.1f);
// CONTROL 회전 수정
FAngleParam param = FAngleParam.MakeAngleParam(current, 0.1f * leftSpeed, -0.1f * rightSpeed, 0.02f, addToDelay, 0);
//회전관련
// WRITE
yawEx[0].Target = AngleConverter.ToDeviceVer2(param.TargetDegree);
yawEx[0].Delay = param.Delay;
}
}
protected override bool GreaterThan(Angle angle)
{
return(this.value > AngleConverter.Convert(angle));
}
public void ConvertDegreesToRadians_ShouldReturnValidValue()
{
Assert.Equal(0, AngleConverter.ConvertDegreesToRadians(0));
Assert.True(Math.Abs(Math.PI / 2 - AngleConverter.ConvertDegreesToRadians(90.0f)) <= 0.0000001);
Assert.True(Math.Abs(-Math.PI * 2 - AngleConverter.ConvertDegreesToRadians(-360.0)) <= 0.0000001);
}
public LinearGradientConverter(Boolean repeating)
: base(repeating)
{
_converter = AngleConverter.Or(
SideOrCornerConverter.StartsWithKeyword(Keywords.To));
}
protected override Angle Add(Angle angle)
{
return(new Degree(this.Value + AngleConverter.Convert(angle)));
}
public LinearGradientConverter()
{
_converter = AngleConverter.Or(
SideOrCornerConverter.StartsWithKeyword(Keywords.To));
}
public void Setup()
{
converter = new AngleConverter();
}
private void Convert(double value)
{
switch (SelectedOp)
{
case "Length":
{
LengthunitConverter unit = new LengthunitConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Mass and Weight":
{
MassConverter unit = new MassConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Power":
{
PowerConverter unit = new PowerConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Pressure":
{
PressureConverter unit = new PressureConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Energy":
{
EnergyConveter unit = new EnergyConveter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Temperature":
{
TemperatureConverter unit = new TemperatureConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Volume":
{
VolumeConverter unit = new VolumeConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Angle":
{
AngleConverter unit = new AngleConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Area":
{
AreaConverter unit = new AreaConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Speed":
{
SpeedConverter unit = new SpeedConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
case "Time":
{
TimeunitsConverter unit = new TimeunitsConverter();
Result = unit.Convert(value, SelectedFrom, SelectedTo).ToString();
break;
}
}
}
protected override Angle Sub(Angle angle)
{
return(new Radiant(this.value - AngleConverter.Convert(angle)));
}
protected override bool Equal(Angle angle)
{
return(this.Value == AngleConverter.Convert(angle));
}
protected override bool Lessthan(Angle angle)
{
return(this.value < AngleConverter.Convert(angle));
}
private static bool DoesFacetNeedSupported(Polygon3 facet, double criticalAngle)
{
return(AngleConverter.RadToDeg(Vector3.CalculateAngle(facet.Normal, XYNormal)) > 180 - criticalAngle);
}
void Thread2_UpdateDevice()
{
ExtendedOutput[] temp = new ExtendedOutput[1];
while (enableThread)
{
//uint inPitch = basic[0].Pitch;
//uint inYaw = yawEx[0].Target;
//uint yawDelay = yawEx[0].Delay;
//// Call Device API
//ExtendedUpdate(basic, rollEx, pitchEx, yawEx, devOutput, devInput, temp);
//uint outPitch = temp[0].Src.Pitch * 8;
//uint outYaw = temp[0].Src.YawEx;
//output.Pitch = 0.7 * Mathf.Abs((int)inPitch - (int)outPitch) / 20000 < 0.125 ? (inPitch + outPitch) / 2 : outPitch;
//float test = AngleConverter.ModifyOutputYaw(
// (float)AngleConverter.ToTypicalVer2(inYaw),
// (int)yawDelay,
// (float)AngleConverter.ToTypicalVer2(outYaw));
//output.Yaw = AngleConverter.ToDeviceVer2(test);
uint inPitch = basic[0].Pitch;
uint inYaw = yawEx[0].Target;
uint yawDelay = yawEx[0].Delay;
// Call Device API
ExtendedUpdate(basic, rollEx, pitchEx, yawEx, devOutput, devInput, temp);
uint outPitch = temp[0].Src.Pitch * 8;
uint outYaw = temp[0].Src.YawEx;
/// 이번에 목표지점 도달시 거리 수정
// Update Tick : 1/8 sec
// Pitch Max Speed : 20000 / 0.7 sec
// Yaw Max Speed : 36000 / 6.8 sec
output.Pitch = 0.7 * Mathf.Abs((int)inPitch - (int)outPitch) / 20000 < 0.125 ?
(inPitch + outPitch) / 2 :
outPitch;
/// 원래 버전
if (version == 1)
{
output.Yaw = 6.8 * Mathf.Abs((int)inYaw - (int)outYaw) / 36000 < 0.125 ?
(inYaw + outYaw) / 2 :
outYaw;
///Debug.Log(" before = " + outYaw + "\t next = " + output.Yaw + "\t delay = " + yawDelay + "\t dir = " + ((int)output.Yaw - (int)outYaw));
}
/// 미리 회전 버전
if (version == 2)
{
// Debug.Log("WHILE IN!!!!!!!!!!!!!!!");
float test = AngleConverter.ModifyOutputYaw(
(float)AngleConverter.ToTypicalVer2(inYaw),
(int)yawDelay,
(float)AngleConverter.ToTypicalVer2(outYaw));
output.Yaw = AngleConverter.ToDeviceVer2(test);
}
}
Debug.Log(" ---- END ---- ");
}
protected override bool NotEqual(Angle angle)
{
return(this.value != AngleConverter.Convert(angle));
}
