public CogConfiguration(TtcCog upperCog, TtcCog lowerCog)
{
UpperCogAngle = upperCog._angle;
UpperCogCurrentAngularVelocity = upperCog._currentAngularVelocity;
UpperCogTargetAngularVelocity = upperCog._targetAngularVelocity;
LowerCogAngle = lowerCog._angle;
LowerCogCurrentAngularVelocity = lowerCog._currentAngularVelocity;
LowerCogTargetAngularVelocity = lowerCog._targetAngularVelocity;
}
public int?FindIdealCogConfiguration(int numFramesMin, int numFramesMax)
{
TtcCog upperCog = _rngObjects[31] as TtcCog;
TtcCog lowerCog = _rngObjects[32] as TtcCog;
List <CogConfiguration> cogConfigurations = new List <CogConfiguration>();
List <int> goodUpperCogAngles = new List <int>()
{
46432, 57360, 2752, 13664, 24592, 35536
};
List <int> goodLowerCogAngles = new List <int>()
{
42576, 53504, 64416, 9808, 20736, 31648
};
List <int> goodLowerCogAnglesAdjusted = goodLowerCogAngles.ConvertAll(angle => angle + 32);
List <int> upperCogEndingYaws = new List <int>()
{
46432, 45232
};
int numCogConfigurations = 9;
int lowerCogGoodAngle = 62988;
List <int> lowerCogGoodAngles = Enumerable.Range(0, 6).ToList()
.ConvertAll(index => lowerCogGoodAngle + 65536 / 6 * index)
.ConvertAll(angle => (int)MoreMath.NormalizeAngleUshort(angle));
//iterate through frames to update objects
int frame = _startingFrame;
int counter = 0;
while (frame < _startingFrame + numFramesMax)
{
frame++;
counter++;
foreach (TtcObject rngObject in _rngObjects)
{
rngObject.SetFrame(frame);
rngObject.Update();
}
if (cogConfigurations.Count >= numCogConfigurations)
{
cogConfigurations.RemoveAt(0);
}
cogConfigurations.Add(new CogConfiguration(upperCog, lowerCog));
if (counter >= numFramesMin)
{
if (cogConfigurations.Count < numCogConfigurations)
{
continue;
}
int upperCogAngleDist = goodUpperCogAngles.Min(
angle => (int)MoreMath.GetAngleDistance(
angle, MoreMath.NormalizeAngleTruncated(cogConfigurations[8].UpperCogAngle)));
int lowerCogAngleDist = goodLowerCogAnglesAdjusted.Min(
angle => (int)MoreMath.GetAngleDistance(
angle, MoreMath.NormalizeAngleTruncated(cogConfigurations[5].LowerCogAngle)));
int lowerCogMinAngularVelocity = 50;
int lowerCogMaxAngularVelocity = 400;
if (upperCogAngleDist == 0 &&
cogConfigurations[8].UpperCogAngle == 46432 &&
cogConfigurations[7].UpperCogTargetAngularVelocity == 1200 &&
cogConfigurations[8].UpperCogCurrentAngularVelocity == 1200 &&
cogConfigurations[0].UpperCogTargetAngularVelocity == 1200 &&
lowerCogAngleDist <= 48 &&
cogConfigurations[2].LowerCogCurrentAngularVelocity <= 200 &&
cogConfigurations[3].LowerCogCurrentAngularVelocity <= 200 &&
cogConfigurations[4].LowerCogCurrentAngularVelocity <= 200 &&
cogConfigurations[5].LowerCogCurrentAngularVelocity <= 200 &&
cogConfigurations[2].LowerCogCurrentAngularVelocity >= 50 &&
cogConfigurations[3].LowerCogCurrentAngularVelocity >= 50 &&
cogConfigurations[4].LowerCogCurrentAngularVelocity >= 50 &&
cogConfigurations[5].LowerCogCurrentAngularVelocity >= 50
/*
* cogConfigurations[2].LowerCogCurrentAngularVelocity == 200 &&
* cogConfigurations[3].LowerCogCurrentAngularVelocity == 150 &&
* cogConfigurations[4].LowerCogCurrentAngularVelocity == 100 &&
* cogConfigurations[5].LowerCogCurrentAngularVelocity == 50
*/)
{
return(frame);
}
}
//if (!upperCogEndingYaws.Any(yaw => yaw == upperCog._endingYaw))
// return null;
}
return(null);
}
public int?FindIdealCogConfiguration(int numFramesMin, int numFramesMax)
{
TtcCog upperCog = _rngObjects[31] as TtcCog;
TtcCog lowerCog = _rngObjects[32] as TtcCog;
List <CogConfiguration> cogConfigurations = new List <CogConfiguration>();
int numCogConfigurations = 9;
int lowerCogGoodAngle = 9892;
List <int> lowerCogGoodAngles = Enumerable.Range(0, 6).ToList()
.ConvertAll(index => lowerCogGoodAngle + 65536 / 6 * index)
.ConvertAll(angle => (int)MoreMath.NormalizeAngleTruncated(angle));
//iterate through frames to update objects
int frame = _startingFrame;
int counter = 0;
while (frame < _startingFrame + numFramesMax)
{
frame++;
counter++;
foreach (TtcObject rngObject in _rngObjects)
{
rngObject.SetFrame(frame);
rngObject.Update();
}
if (cogConfigurations.Count >= numCogConfigurations)
{
cogConfigurations.RemoveAt(0);
}
cogConfigurations.Add(new CogConfiguration(upperCog, lowerCog));
if (counter >= numFramesMin)
{
if (cogConfigurations.Count < numCogConfigurations)
{
continue;
}
int lowerCogAngleDist = lowerCogGoodAngles.Min(
angle => (int)MoreMath.GetAngleDistance(
angle, MoreMath.NormalizeAngleTruncated(cogConfigurations[5].LowerCogAngle)));
bool upperCogPreGoal =
cogConfigurations[8].UpperCogAngle == 46432 && // right angle
cogConfigurations[7].UpperCogTargetAngularVelocity == 1200 && // was targeting 1200
cogConfigurations[8].UpperCogCurrentAngularVelocity == 1200; // moved at 1200 speed
bool upperCogGoal =
cogConfigurations[8].UpperCogAngle == 46432 && // right angle
cogConfigurations[7].UpperCogTargetAngularVelocity == 1200 && // was targeting 1200
cogConfigurations[8].UpperCogCurrentAngularVelocity == 1200 && // moved at 1200 speed
cogConfigurations[0].UpperCogTargetAngularVelocity == 1200; // had been targeting 1200 for some time
int lowerCogMinAngularVelocity = 0;
int lowerCogMaxAngularVelocity = 400;
bool lowerCogGoal =
lowerCogAngleDist <= 64 && // close to some right angle
// was moving slowly leading up to right angle
cogConfigurations[1].LowerCogCurrentAngularVelocity <= lowerCogMaxAngularVelocity &&
cogConfigurations[2].LowerCogCurrentAngularVelocity <= lowerCogMaxAngularVelocity &&
cogConfigurations[3].LowerCogCurrentAngularVelocity <= lowerCogMaxAngularVelocity &&
cogConfigurations[4].LowerCogCurrentAngularVelocity <= lowerCogMaxAngularVelocity &&
cogConfigurations[5].LowerCogCurrentAngularVelocity <= lowerCogMaxAngularVelocity &&
cogConfigurations[1].LowerCogCurrentAngularVelocity >= lowerCogMinAngularVelocity &&
cogConfigurations[2].LowerCogCurrentAngularVelocity >= lowerCogMinAngularVelocity &&
cogConfigurations[3].LowerCogCurrentAngularVelocity >= lowerCogMinAngularVelocity &&
cogConfigurations[4].LowerCogCurrentAngularVelocity >= lowerCogMinAngularVelocity &&
cogConfigurations[5].LowerCogCurrentAngularVelocity >= lowerCogMinAngularVelocity;
if (upperCogGoal && lowerCogGoal)
//if (upperCogPreGoal)
{
return(frame);
}
}
//if (!upperCogEndingYaws.Any(yaw => yaw == upperCog._endingYaw))
// return null;
}
return(null);
}