public static void PrintResult(string tag)
{
if (profiles.ContainsKey(tag))
{
System.Text.StringBuilder output = new System.Text.StringBuilder();
ProfilePoint profileTime = profiles [tag];
double totalTime = profileTime.totalTime.TotalSeconds;
int totalCalls = profileTime.totalCalls;
output.Append("\nProfile ");
output.Append(tag);
output.Append(" took ");
output.Append(totalTime.ToString("F9"));
output.Append(" seconds to complete over ");
output.Append(totalCalls);
output.Append(" iteration");
if (totalCalls != 1)
{
output.Append("s");
}
output.Append(", averaging ");
output.Append((totalTime / totalCalls).ToString("F9"));
output.Append(" seconds per call");
UnityEngine.Debug.Log(output.ToString());
profiles.Remove(tag);
}
}
public static void PrintFastResults()
{
if (fastProfiles == null)
{
return;
}
StartFastProfile(fastProfiles.Length - 2);
for (int i = 0; i < 1000; i++)
{
StartFastProfile(fastProfiles.Length - 1);
EndFastProfile(fastProfiles.Length - 1);
}
EndFastProfile(fastProfiles.Length - 2);
double avgOverhead = fastProfiles[fastProfiles.Length - 2].watch.Elapsed.TotalMilliseconds / 1000.0;
TimeSpan endTime = DateTime.UtcNow - startTime;
var output = new System.Text.StringBuilder();
output.Append("============================\n\t\t\t\tProfile results:\n============================\n");
output.Append("Name | Total Time | Total Calls | Avg/Call | Bytes");
//foreach(KeyValuePair<string, ProfilePoint> pair in profiles)
for (int i = 0; i < fastProfiles.Length; i++)
{
string name = fastProfileNames[i];
ProfilePoint value = fastProfiles[i];
int totalCalls = value.totalCalls;
double totalTime = value.watch.Elapsed.TotalMilliseconds - avgOverhead * totalCalls;
if (totalCalls < 1)
{
continue;
}
output.Append("\n").Append(name.PadLeft(10)).Append("| ");
output.Append(totalTime.ToString("0.0 ").PadLeft(10)).Append(value.watch.Elapsed.TotalMilliseconds.ToString("(0.0)").PadLeft(10)).Append("| ");
output.Append(totalCalls.ToString().PadLeft(10)).Append("| ");
output.Append((totalTime / totalCalls).ToString("0.000").PadLeft(10));
/* output.Append("\nProfile");
* output.Append(name);
* output.Append(" took \t");
* output.Append(totalTime.ToString("0.0"));
* output.Append(" ms to complete over ");
* output.Append(totalCalls);
* output.Append(" iteration");
* if (totalCalls != 1) output.Append("s");
* output.Append(", averaging \t");
* output.Append((totalTime / totalCalls).ToString("0.000"));
* output.Append(" ms per call"); */
}
output.Append("\n\n============================\n\t\tTotal runtime: ");
output.Append(endTime.TotalSeconds.ToString("F3"));
output.Append(" seconds\n============================");
Debug.Log(output.ToString());
}
public static List <ProfileLine> ConvertEsriPolylineToLine(List <IPolyline> polylines)
{
var id = 0;
var spatialReference = polylines[0].SpatialReference;
var esriPolylines = new List <IPolyline>(polylines);
return(esriPolylines.Select(line =>
{
id++;
line.Project(EsriTools.Wgs84Spatialreference);
var pointFrom = new ProfilePoint {
SpatialReference = line.SpatialReference, X = line.FromPoint.X, Y = line.FromPoint.Y
};
var pointTo = new ProfilePoint {
SpatialReference = line.SpatialReference, X = line.ToPoint.X, Y = line.ToPoint.Y
};
line.Project(spatialReference);
return new ProfileLine
{
Line = line,
Id = id,
PointFrom = pointFrom,
PointTo = pointTo,
};
}
).ToList());
}
public static void EndFastProfile(int tag)
{
ProfilePoint point = fastProfiles[tag];
point.totalCalls++;
point.watch.Stop();
}
public static void PrintFastResults()
{
if (fastProfiles != null)
{
for (int i = 0; i < 0x3e8; i++)
{
}
double num2 = fastProfiles[fastProfiles.Length - 2].watch.Elapsed.TotalMilliseconds / 1000.0;
TimeSpan span = (TimeSpan)(DateTime.UtcNow - startTime);
StringBuilder builder = new StringBuilder();
builder.Append("============================\n\t\t\t\tProfile results:\n============================\n");
builder.Append("Name\t\t|\tTotal Time\t|\tTotal Calls\t|\tAvg/Call\t|\tBytes");
for (int j = 0; j < fastProfiles.Length; j++)
{
string str = fastProfileNames[j];
ProfilePoint point = fastProfiles[j];
int totalCalls = point.totalCalls;
double num5 = point.watch.Elapsed.TotalMilliseconds - (num2 * totalCalls);
if (totalCalls >= 1)
{
builder.Append("\n").Append(str.PadLeft(10)).Append("| ");
builder.Append(num5.ToString("0.0 ").PadLeft(10)).Append(point.watch.Elapsed.TotalMilliseconds.ToString("(0.0)").PadLeft(10)).Append("| ");
builder.Append(totalCalls.ToString().PadLeft(10)).Append("| ");
double num7 = num5 / ((double)totalCalls);
builder.Append(num7.ToString("0.000").PadLeft(10));
}
}
builder.Append("\n\n============================\n\t\tTotal runtime: ");
builder.Append(span.TotalSeconds.ToString("F3"));
builder.Append(" seconds\n============================");
UnityEngine.Debug.Log(builder.ToString());
}
}
public static void InitializeFastProfile (string[] profileNames) {
fastProfileNames = new string[profileNames.Length+2];
Array.Copy (profileNames,fastProfileNames, profileNames.Length);
fastProfileNames[fastProfileNames.Length-2] = "__Control1__";
fastProfileNames[fastProfileNames.Length-1] = "__Control2__";
fastProfiles = new ProfilePoint[fastProfileNames.Length];
for (int i=0;i<fastProfiles.Length;i++) fastProfiles[i] = new ProfilePoint();
}
ProfilePoint[] TransProfileData(KObject rawData)
{
ProfilePoint[] profileBuffer = null;
GoDataSet _dataSource = new GoDataSet();
_dataSource = (GoDataSet)rawData;
_dataContext = new DataContext();
ProfileShape _profileShape = new ProfileShape();
for (UInt32 i = 0; i < _dataSource.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)_dataSource.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.ResampledProfile:
{
GoResampledProfileMsg profileMsg = (GoResampledProfileMsg)dataObj;
_dataContext.xResolution = (double)profileMsg.XResolution / 1000000;
_dataContext.zResolution = (double)profileMsg.ZResolution / 1000000;
_dataContext.xOffset = (double)profileMsg.XOffset / 1000;
_dataContext.zOffset = (double)profileMsg.ZOffset / 1000;
_profileShape.width = profileMsg.Width;
_profileShape.size = profileMsg.Size;
_syncContext.Post(delegate
{
xRes.Content = _dataContext.xResolution.ToString();
zRes.Content = _dataContext.zResolution.ToString();
}, null);
//generate csv file for point data save
short[] points = new short[_profileShape.width];
profileBuffer = new ProfilePoint[_profileShape.width];
IntPtr pointsPtr = profileMsg.Data;
Marshal.Copy(pointsPtr, points, 0, points.Length);
for (UInt32 arrayIndex = 0; arrayIndex < _profileShape.width; ++arrayIndex)
{
if (points[arrayIndex] != -32768)
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = _dataContext.zOffset + _dataContext.zResolution * points[arrayIndex];
}
else
{
profileBuffer[arrayIndex].x = _dataContext.xOffset + _dataContext.xResolution * arrayIndex;
profileBuffer[arrayIndex].z = activeArea.Top;
}
}
}
break;
default: break;
}
}
return(profileBuffer);
}
public static IPoint GetEsriPoint(this ProfilePoint point, ISpatialReference spatial = null)
{
if (point == null)
{
return(null);
}
return(new Point()
{
X = point.X, Y = point.Y, SpatialReference = spatial
});
}
public static void Reset()
{
profiles.Clear();
startTime = DateTime.UtcNow;
if (fastProfiles != null)
{
for (int i = 0; i < fastProfiles.Length; i++)
{
fastProfiles[i] = new ProfilePoint();
}
}
}
public static void InitializeFastProfile(string[] profileNames)
{
fastProfileNames = new string[profileNames.Length + 2];
Array.Copy(profileNames, fastProfileNames, profileNames.Length);
fastProfileNames[fastProfileNames.Length - 2] = "__Control1__";
fastProfileNames[fastProfileNames.Length - 1] = "__Control2__";
fastProfiles = new ProfilePoint[fastProfileNames.Length];
for (int i = 0; i < fastProfiles.Length; i++)
{
fastProfiles[i] = new ProfilePoint();
}
}
public static void EndProfile(string tag)
{
if (!profiles.ContainsKey(tag))
{
Debug.Log("Can only end profiling for a tag which has already been started (tag was " + tag + ")");
return;
}
ProfilePoint point = profiles[tag];
point.totalTime += DateTime.UtcNow - point.lastRecorded;
++point.totalCalls;
profiles[tag] = point;
}
public static void StartProfile(string tag)
{
ProfilePoint point;
profiles.TryGetValue(tag, out point);
if (point == null)
{
point = new ProfilePoint();
profiles[tag] = point;
}
point.tmpBytes = GC.GetTotalMemory(false);
point.watch.Start();
}
/// <summary>
/// Subdivide profile points according to the max diameter change.
/// </summary>
private void SubdivHorizontal(LinkedList <ProfilePoint> pts)
{
ProfilePoint prev = pts.First.Value;
for (LinkedListNode <ProfilePoint> node = pts.First.Next; node != null; node = node.Next)
{
ProfilePoint curr = node.Value;
if (!curr.inRender)
{
continue;
}
float dDiameter = curr.dia - prev.dia;
float dPercentage = Math.Abs(curr.dia - prev.dia) / (Math.Max(curr.dia, prev.dia) / 100.0f);
int subdiv = Math.Min((int)(Math.Truncate(dPercentage / MaxDiameterChange)), 30);
//int subdiv = Math.Min((int)Math.Truncate(Mathf.Abs(dDiameter) / MaxDiameterChange), 30);
if (subdiv > 1)
{
// slerp alg for normals http://http://en.wikipedia.org/wiki/Slerp
bool doSlerp = prev.norm != curr.norm;
float omega = 0, sinOmega = 0;
if (doSlerp)
{
omega = Mathf.Acos(Vector2.Dot(prev.norm, curr.norm));
sinOmega = Mathf.Sin(omega);
}
for (int i = 1; i < subdiv; ++i)
{
float t = i / (float)subdiv;
float tDiameter = prev.dia + dDiameter * t;
float tY = Mathf.Lerp(prev.y, curr.y, t);
float tV = Mathf.Lerp(prev.v, curr.v, t);
Vector2 norm;
if (doSlerp)
{
norm = (Mathf.Sin(omega * (1f - t)) / sinOmega * prev.norm + Mathf.Sin(omega * t) / sinOmega * curr.norm);
}
else
{
norm = prev.norm;
}
pts.AddBefore(node, new ProfilePoint(dia: tDiameter, y: tY, v: tV, norm: norm, inCollider: false));
}
}
prev = curr;
}
}
public static void EndProfile(string tag)
{
if (!profiles.ContainsKey(tag))
{
UnityEngine.Debug.LogError("Can only end profiling for a tag which has already been started (tag was " + tag + ")");
}
else
{
ProfilePoint point = profiles[tag];
point.totalCalls++;
point.watch.Stop();
point.totalBytes += GC.GetTotalMemory(false) - point.tmpBytes;
}
}
public static void StartProfile (string tag) {
//Console.WriteLine ("Profile Start - " + tag);
ProfilePoint point;
profiles.TryGetValue(tag, out point);
if (point == null) {
point = new ProfilePoint();
profiles[tag] = point;
}
point.tmpBytes = GC.GetTotalMemory(false);
point.watch.Start();
//point.lastRecorded = DateTime.UtcNow;
//Debug.Log ("Starting " + tag);
}
public static void EndFastProfile(int tag)
{
DateTime now = DateTime.UtcNow;
/*if (!profiles.ContainsKey(tag))
* {
* Debug.LogError("Can only end profiling for a tag which has already been started (tag was " + tag + ")");
* return;
* }*/
ProfilePoint point = fastProfiles[tag];
point.totalTime += now - point.lastRecorded;
point.totalCalls++;
fastProfiles[tag] = point;
}
public Vector3[] GetEndcapVerticies(bool top)
{
if (lastProfile == null)
{
return(new Vector3[0]);
}
ProfilePoint profilePoint = top ? lastProfile.Last.Value : lastProfile.First.Value;
Vector3[] verticies = new Vector3[profilePoint.circ.totVertexes];
bool odd = lastProfile.Count % 2 == 0;
profilePoint.circ.WriteEndcapVerticies(profilePoint.dia, profilePoint.y, 0, verticies, odd);
return(verticies);
}
public static void StartProfile(string tag)
{
//Console.WriteLine ("Profile Start - " + tag);
ProfilePoint point;
profiles.TryGetValue(tag, out point);
if (point == null)
{
point = new ProfilePoint();
profiles[tag] = point;
}
point.tmpBytes = System.GC.GetTotalMemory(false);
point.watch.Start();
//point.lastRecorded = DateTime.UtcNow;
//Debug.Log ("Starting " + tag);
}
public override object ReadJson(JsonReader reader, Type objectType, object existingValue,
JsonSerializer serializer)
{
// Load the JSON for the Result into a JObject
var jo = JObject.Load(reader);
// Read the properties which will be used as constructor parameters
var jValue = (JObject)jo[nameof(ProfilePoint.Position)];
var vect = serializer.Deserialize <VVector>(jValue.CreateReader());
var value = (double)jo[nameof(ProfilePoint.Value)];
// Construct the Result object using the non-default constructor
var pp = new ProfilePoint(vect, value);
// Return the result
return(pp);
}
List <ProfilePoint[]> OutputKeepRatioGradient(List <ProfilePoint[]> rawData, double rawRatio)
{
List <ProfilePoint[]> newData = new List <ProfilePoint[]>();
int ratio = (int)Math.Ceiling(rawRatio);
//处理前Count-1数据
for (int i = 0; i < rawData.Count - 1; i++)
{
double[] delta = new double[rawData[0].Length];
for (int j = 0; j < rawData[0].Length; j++)
{
delta[j] = rawData[i][j].z - rawData[i + 1][j].z;
}
for (int k = 0; k < ratio; k++)
{
ProfilePoint[] temp = new ProfilePoint[rawData[0].Length];
for (int w = 0; w < temp.Length; w++)
{
temp[w].x = rawData[i][w].x;
temp[w].z = rawData[i][w].z - k * delta[w] / ratio;
}
newData.Add(temp);
}
}
//处理last stride
double[] alpha = new double[rawData[0].Length];
for (int j = 0; j < rawData[0].Length; j++)
{
alpha[j] = rawData[rawData.Count - 1][j].z - 0;
}
for (int k = 0; k < ratio; k++)
{
ProfilePoint[] temp = new ProfilePoint[rawData[0].Length];
for (int w = 0; w < temp.Length; w++)
{
temp[w].x = rawData[rawData.Count - 1][w].x;
temp[w].z = rawData[rawData.Count - 1][w].z - k * alpha[w] / ratio;
}
newData.Add(temp);
}
return(newData);
}
public static void EndProfile(string tag)
{
if (!profiles.ContainsKey(tag))
{
Debug.LogError("Can only end profiling for a tag which has already been started (tag was " + tag + ")");
return;
}
//Console.WriteLine ("Profile End - " + tag);
//DateTime now = DateTime.UtcNow;
ProfilePoint point = profiles[tag];
//point.totalTime += now - point.lastRecorded;
++point.totalCalls;
point.watch.Stop();
point.totalBytes += System.GC.GetTotalMemory(false) - point.tmpBytes;
//profiles[tag] = point;
//Debug.Log ("Ending " + tag);
}
public static void EndProfile(string tag)
{
#if !UNITY_PRO_PROFILER
if (!profiles.ContainsKey(tag))
{
Debug.LogError("Can only end profiling for a tag which has already been started (tag was " + tag + ")");
return;
}
//Console.WriteLine ("Profile End - " + tag);
DateTime now = DateTime.UtcNow;
ProfilePoint point = profiles[tag];
point.totalTime += now - point.lastRecorded;
++point.totalCalls;
profiles[tag] = point;
#else
EndProfile();
#endif
}
public static void StartProfile(string tag)
{
#if UNITY_PRO_PROFILER
Profiler.BeginSample (tag);
#else
//Console.WriteLine ("Profile Start - " + tag);
ProfilePoint point;
profiles.TryGetValue(tag, out point);
if (point == null) {
point = new ProfilePoint();
profiles[tag] = point;
}
point.tmpBytes = System.GC.GetTotalMemory (false);
point.watch.Start();
//point.lastRecorded = DateTime.UtcNow;
//Debug.Log ("Starting " + tag);
#endif
}
public static void PrintFastResults()
{
TimeSpan endTime = DateTime.UtcNow - startTime;
System.Text.StringBuilder output = new System.Text.StringBuilder();
output.Append("============================\n\t\t\t\tProfile results:\n============================\n");
output.Append("Name | Total Time | Total Calls | Avg/Call ");
//foreach(KeyValuePair<string, ProfilePoint> pair in profiles)
for (int i = 0; i < fastProfiles.Length; i++)
{
string name = fastProfileNames[i];
ProfilePoint value = fastProfiles[i];
double totalTime = value.totalTime.TotalMilliseconds;
int totalCalls = value.totalCalls;
if (totalCalls < 1)
{
continue;
}
output.Append("\n").Append(name.PadLeft(10)).Append("| ");
output.Append(totalTime.ToString("0.0").PadLeft(10)).Append("| ");
output.Append(totalCalls.ToString().PadLeft(10)).Append("| ");
output.Append((totalTime / totalCalls).ToString("0.000").PadLeft(10));
/* output.Append("\nProfile");
* output.Append(name);
* output.Append(" took \t");
* output.Append(totalTime.ToString("0.0"));
* output.Append(" ms to complete over ");
* output.Append(totalCalls);
* output.Append(" iteration");
* if (totalCalls != 1) output.Append("s");
* output.Append(", averaging \t");
* output.Append((totalTime / totalCalls).ToString("0.000"));
* output.Append(" ms per call"); */
}
output.Append("\n\n============================\n\t\tTotal runtime: ");
output.Append(endTime.TotalSeconds.ToString("F3"));
output.Append(" seconds\n============================");
Debug.Log(output.ToString());
}
private void UpdateNodeSize(ProfilePoint pt, string nodeName)
{
AttachNode node = part.attachNodes.Find(n => n.id == nodeName);
if (node == null)
{
return;
}
node.size = Math.Min((int)(pt.dia / PPart.diameterLargeStep), 3);
// Breaking force and torque scales with the area of the surface (node size).
node.breakingTorque = node.breakingForce = Mathf.Max(50 * node.size * node.size, 50);
// Send messages for the changing of the ends
RaiseChangeAttachNodeSize(node, pt.dia, Mathf.PI * pt.dia * pt.dia * 0.25f);
// TODO: separate out the meshes for each end so we can use the scale for texturing.
RaiseChangeTextureScale(nodeName, PPart.EndsMaterial, new Vector2(pt.dia, pt.dia));
}
public static void StartProfile(string tag)
{
#if ASTAR_UNITY_PRO_PROFILER
Profiler.BeginSample(tag);
#else
//Console.WriteLine ("Profile Start - " + tag);
ProfilePoint point;
profiles.TryGetValue(tag, out point);
if (point == null)
{
point = new ProfilePoint();
profiles[tag] = point;
}
point.tmpBytes = GC.GetTotalMemory(false);
point.watch.Start();
//point.lastRecorded = DateTime.UtcNow;
//Debug.Log ("Starting " + tag);
#endif
}
private static Quaternion SideAttachOrientation(ProfilePoint pv, ProfilePoint pt, float theta, out Vector3 normal)
{
normal = Quaternion.AngleAxis(theta * 180 / Mathf.PI, Vector3.up) * new Vector2(pt.y - pv.y, -(pt.dia - pv.dia) / 2f);
return Quaternion.FromToRotation(Vector3.up, normal);
}
public void onData(KObject data)
{
double yPoint = 1;
string strWrite = "";
bool length_incr = true;
GoDataSet dataSet = (GoDataSet)data;
DataContext context = new DataContext();
for (UInt32 i = 0; i < dataSet.Count; i++)
{
GoDataMsg dataObj = (GoDataMsg)dataSet.Get(i);
switch (dataObj.MessageType)
{
case GoDataMessageType.Stamp:
{
GoStampMsg stampMsg = (GoStampMsg)dataObj;
for (UInt32 j = 0; j < stampMsg.Count; j++)
{
GoStamp stamp = stampMsg.Get(j);
//Console.WriteLine("Frame Index = {0}", stamp.FrameIndex);
//Console.WriteLine("Time Stamp = {0}", stamp.Timestamp);
//Console.WriteLine("Encoder Value = {0}", stamp.Encoder);
}
}
break;
case GoDataMessageType.Surface: // Surface
{
GoSurfaceMsg surfaceMsg = (GoSurfaceMsg)dataObj;
long width = surfaceMsg.Width;
long height = surfaceMsg.Length;
long bufferSize = width * height;
IntPtr bufferPointer = surfaceMsg.Data;
short[] ranges = new short[bufferSize];
Marshal.Copy(bufferPointer, ranges, 0, ranges.Length);
context.xResolution = (double)surfaceMsg.XResolution / 1000000.0;
context.zResolution = (double)surfaceMsg.ZResolution / 1000000.0;
context.yResolution = (double)surfaceMsg.YResolution / 1000000.0;
context.yOffset = (double)surfaceMsg.YOffset / 1000.0;
context.xOffset = (double)surfaceMsg.XOffset / 1000.0;
context.zOffset = (double)surfaceMsg.ZOffset / 1000.0;
double phy_x;
double phy_y;
double phy_z;
FileStream fs = new FileStream(strFileSave, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
for (int m = 0; m < height; m++)
{
for (int j = 0; j < width; j++)
{
phy_z = ranges[m * width + j] * context.zResolution + context.zOffset;
if (/*phy_z > 20*/ true) //这个过滤阈值根据实际情况选取
{
phy_x = j * context.xResolution + context.xOffset;
phy_y = m * context.yResolution + context.yOffset;
strWrite = string.Format("{0} {1} {2}", phy_x, phy_y, phy_z);
sw.WriteLine(strWrite);
}
}
}
sw.Flush();
//关闭流
sw.Close();
fs.Close();
}
break;
case GoDataMessageType.Profile:
{
StreamWriter write = new StreamWriter(strFileSave, true);
GoProfileMsg profileMsg = (GoProfileMsg)dataObj;
Console.WriteLine(" Profile Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
int validPointCount = 0;
long profilePointCount = profileMsg.Width;
Console.WriteLine(" Item[{0}]: Profile data ({1} points)", i, profileMsg.Width);
context.xResolution = (profileMsg.XResolution / 1000000.0);
context.zResolution = profileMsg.ZResolution / 1000000.0;
context.xOffset = profileMsg.XOffset / 1000.0;
context.zOffset = profileMsg.ZOffset / 1000.0;
GoPoints[] points = new GoPoints[profilePointCount];
point[] point111 = new point[profilePointCount];
ProfilePoint[] profileBuffer = new ProfilePoint[profilePointCount];
int structSize = Marshal.SizeOf(typeof(GoPoints));
IntPtr pointsPtr = profileMsg.Data;
for (UInt32 array = 0; array < profilePointCount; ++array)
{
IntPtr incPtr = new IntPtr(pointsPtr.ToInt64() + array * structSize);
points[array] = (GoPoints)Marshal.PtrToStructure(incPtr, typeof(GoPoints));
double real_x = (context.xOffset + context.xResolution * points[array].x);
double real_z = (context.zOffset + context.zResolution * points[array].y);
if (length_incr == true)
{
length += ReadIniSettings.ReadIni.objIniValue.iniScanner.step;
length_incr = false;
}
//if (points[array].x != -32768 && real_z > -30.0)
if (real_z > 20.0 && real_x > -500.0)
{
//if (length_incr == true)
//{
// length++;
// length_incr = false;
//}
//double real_y = offset + (length - 1) * ReadIni.objIniValue.iniScanner.speed * ReadIni.objIniValue.iniScanner.frequency;
write.WriteLine(real_x + " " + length + " " + real_z);
}
}
write.Flush();
}
write.Close();
}
break;
case GoDataMessageType.ProfileIntensity:
{
GoProfileIntensityMsg profileMsg = (GoProfileIntensityMsg)dataObj;
Console.WriteLine(" Profile Intensity Message batch count: {0}", profileMsg.Count);
for (UInt32 k = 0; k < profileMsg.Count; ++k)
{
byte[] intensity = new byte[profileMsg.Width];
IntPtr intensityPtr = profileMsg.Data;
Marshal.Copy(intensityPtr, intensity, 0, intensity.Length);
}
}
break;
case GoDataMessageType.Measurement: // Measurement
{
GoMeasurementMsg measurementMsg = (GoMeasurementMsg)dataObj;
}
break;
}
}
}
public static List <IntersectionLine> ConvertEsriPolylineToIntersectionLines(List <IPolyline> polylines, ProfilePoint pointFrom, LayersEnum layer, double distance)
{
var id = 0;
var fromPoint = new Point {
X = pointFrom.X, Y = pointFrom.Y, SpatialReference = EsriTools.Wgs84Spatialreference
};
return(polylines.Select(line =>
{
id++;
fromPoint.Project(line.SpatialReference);
var fromLength = EsriTools.CreatePolylineFromPoints(fromPoint, line.FromPoint).Length;
var toLength = EsriTools.CreatePolylineFromPoints(fromPoint, line.ToPoint).Length;
double startDistance = (fromLength < toLength) ? fromLength : toLength;
double endDistance = (fromLength > toLength) ? fromLength : toLength;
return new IntersectionLine()
{
PointFromDistance = startDistance + distance,
PointToDistance = endDistance + distance,
LayerType = layer
};
}
).ToList());
}
private object AddAttachmentNotNormalized(TransformFollower attach)
{
Attachment ret = new Attachment
{
follower = attach
};
if (lastProfile == null)
{
throw new InvalidOperationException("Can't child non-normalized attachments prior to the first update");
}
// All the code from here down assumes the part is a convex shape, which is fair as it needs to be convex for
// partCollider purposes anyhow. If we allow concave shapes it will need some refinement.
Vector3 position = attach.transform.localPosition;
// Convert the offset into spherical coords
float r = position.magnitude;
float theta, phi;
if (r > 0f)
{
theta = Mathf.Atan2(-position.z, position.x);
phi = Mathf.Asin(position.y / r);
}
else
{
// move the origin to the top to avoid divide by zeros.
theta = 0;
phi = Mathf.PI / 2f;
}
// top or bottom?
// ReSharper disable once CompareOfFloatsByEqualityOperator
if (phi != 0)
{
ProfilePoint topBot = (phi < 0) ? lastProfile.First.Value : lastProfile.Last.Value;
float tbR = Mathf.Sqrt(topBot.y * topBot.y + topBot.dia * topBot.dia * 0.25f);
float tbPhi = Mathf.Asin(topBot.y / tbR);
if (Mathf.Abs(phi) >= Mathf.Abs(tbPhi))
{
ret.uv = topBot.dia < 0.001f ?
new Vector2(0.5f, 0.5f) :
new Vector2(position.x / topBot.dia * 2f + 0.5f, position.z / topBot.dia * 2f + 0.5f);
if (phi > 0)
{
ret.location = Location.Top;
ret.node = topAttachments.AddLast(ret);
ret.follower.SetLocalRotationReference(Quaternion.LookRotation(Vector3.up, Vector3.right));
}
else
{
ret.location = Location.Bottom;
ret.node = bottomAttachments.AddLast(ret);
ret.follower.SetLocalRotationReference(Quaternion.LookRotation(Vector3.down, Vector3.left));
}
//Debug.LogWarning("Adding non-normalized attachment to position=" + position + " location=" + ret.location + " uv=" + ret.uv + " attach=" + attach.name);
return(ret);
}
}
// THis is the slope of a line projecting out towards our attachment
float s = position.y / Mathf.Sqrt(position.x * position.x + position.z * position.z);
ret.location = Location.Side;
ret.uv[0] = (Mathf.InverseLerp(-Mathf.PI, Mathf.PI, theta) + 0.5f) % 1.0f;
ProfilePoint pt = lastProfile.First.Value;
for (LinkedListNode <ProfilePoint> ptNode = lastProfile.First.Next; ptNode != null; ptNode = ptNode.Next)
{
if (!ptNode.Value.inCollider)
{
continue;
}
ProfilePoint pv = pt;
pt = ptNode.Value;
float ptR = Mathf.Sqrt(pt.y * pt.y + pt.dia * pt.dia * 0.25f);
float ptPhi = Mathf.Asin(pt.y / ptR);
//Debug.LogWarning("ptPhi=" + ptPhi + " phi=" + phi);
if (phi > ptPhi)
{
continue;
}
// so we know the attachment is somewhere between the previous and this circle
// Geometry: draw a line between the point (dia/2, y) in the prev circle and (dia/2, y) in the current circle (parametric in t)
// find the point on the line where y = s * dia / 2 and solve for t
// r(t) = r0 + (r1-r0)t
// y(t) = y0 + (y1-y0)t
// y(t) = s * r(t)
//
// y0 + (y1-y0)t = s r0 + s (r1-r0) t
// ((y1-y0)- s(r1-r0))t = s r0 - y0
// t = (s r0 - y0) / ((y1-y0) - s(r1-r0))
float r0 = pv.dia * 0.5f;
float r1 = pt.dia * 0.5f;
float t = (s * r0 - pv.y) / ((pt.y - pv.y) - s * (r1 - r0));
//Debug.LogWarning(string.Format("New Attachment: pv=({0:F2}, {1:F2}) pt=({2:F2}, {3:F2}) s={4:F2} t={5:F2}", r0, pv.y, r1, pt.y, s, t));
ret.uv[1] = Mathf.Lerp(pv.v, pt.v, t);
if (ret.uv[1] > 1.0f)
{
Debug.LogError("result off end of segment v=" + ret.uv[1] + " pv.v=" + pv.v + " pt.v=" + pt.v + " t=" + t);
}
//
Vector3 normal;
Quaternion rot = SideAttachOrientation(pv, pt, theta, out normal);
ret.follower.SetLocalRotationReference(rot);
AddSideAttachment(ret);
//Debug.LogWarning("Adding non-normalized attachment to position=" + position + " location=" + ret.location + " uv=" + ret.uv + " attach=" + attach.name);
return(ret);
}
// This should be impossible to reach
throw new InvalidProgramException("Unreachable code reached");
}
/// <summary>
/// Generate the compShape from profile points from pt to bottom.
/// Note that this list will have extra interpolated points added if the change in radius is high to avoid
/// texture stretching.
/// </summary>
/// <param name="pts"></param>
protected void WriteMeshes(LinkedList <ProfilePoint> pts)
{
if (pts == null || pts.Count < 2)
{
return;
}
// update nodes
UpdateNodeSize(pts.First(), bottomNodeName);
UpdateNodeSize(pts.Last(), topNodeName);
// Move attachments first, before subdividing
MoveAttachments(pts);
// Horizontal profile point subdivision
SubdivHorizontal(pts);
// Tank stats
float tankVLength = 0;
int nVrt = 0;
int nTri = 0;
int nColVrt = 0;
int nColTri = 0;
bool customCollider = false;
ProfilePoint first = pts.First.Value;
ProfilePoint last = pts.Last.Value;
if (!first.inCollider || !last.inCollider)
{
throw new InvalidOperationException("First and last profile points must be used in the collider");
}
foreach (ProfilePoint pt in pts)
{
customCollider = customCollider || pt.CustomCollider;
if (pt.inRender)
{
nVrt += pt.circ.totVertexes + 1;
// one for above, one for below
nTri += 2 * pt.circ.totVertexes;
}
if (pt.inCollider)
{
nColVrt += pt.colliderCirc.totVertexes + 1;
nColTri += 2 * pt.colliderCirc.totVertexes;
}
}
// Have double counted for the first and last circles.
nTri -= first.circ.totVertexes + last.circ.totVertexes;
nColTri -= first.colliderCirc.totVertexes + last.colliderCirc.totVertexes;
UncheckedMesh m = new UncheckedMesh(nVrt, nTri);
float sumDiameters = 0;
//Debug.LogWarning("Display mesh vert=" + nVrt + " tris=" + nTri);
bool odd = false;
{
ProfilePoint prev = null;
int off = 0, prevOff = 0;
int tOff = 0;
foreach (ProfilePoint pt in pts)
{
if (!pt.inRender)
{
continue;
}
pt.circ.WriteVertexes(diameter: pt.dia, y: pt.y, v: pt.v, norm: pt.norm, off: off, m: m, odd: odd);
if (prev != null)
{
CirclePoints.WriteTriangles(prev.circ, prevOff, pt.circ, off, m.triangles, tOff * 3, !odd);
tOff += prev.circ.totVertexes + pt.circ.totVertexes;
// Deprecated: Volume has been moved up to callers. This way we can use the idealized rather than aproximate volume
// Work out the area of the truncated cone
// integral_y1^y2 pi R(y)^2 dy where R(y) = ((r2-r1)(y-y1))/(r2-r1) + r1 Integrate circles along a line
// integral_y1^y2 pi ( ((r2-r1)(y-y1))/(r2-r1) + r1) ^2 dy Substituted in formula.
// == -1/3 pi (y1-y2) (r1^2+r1*r2+r2^2) Do the calculus
// == -1/3 pi (y1-y2) (d1^2/4+d1*d2/4+d2^2/4) r = d/2
// == -1/12 pi (y1-y2) (d1^2+d1*d2+d2^2) Take out the factor
//volume += (Mathf.PI * (pt.y - prev.y) * (prev.dia * prev.dia + prev.dia * pt.dia + pt.dia * pt.dia)) / 12f;
float dy = (pt.y - prev.y);
float dr = (prev.dia - pt.dia) * 0.5f;
//print("dy=" + dy + " dr=" + dr + " len=" + Mathf.Sqrt(dy * dy + dr * dr).ToString("F3"));
tankVLength += Mathf.Sqrt(dy * dy + dr * dr);
// average diameter weighted by dy
sumDiameters += (pt.dia + prev.dia) * dy;
}
prev = pt;
prevOff = off;
off += pt.circ.totVertexes + 1;
odd = !odd;
}
}
// Use the weighted average diameter across segments to set the ULength
float tankULength = Mathf.PI * sumDiameters / (last.y - first.y);
//print("ULength=" + tankULength + " VLength=" + tankVLength);
// set the texture scale.
RaiseChangeTextureScale("sides", PPart.SidesMaterial, new Vector2(tankULength, tankVLength));
m.WriteTo(SidesMesh);
// The endcaps.
nVrt = first.circ.totVertexes + last.circ.totVertexes;
nTri = first.circ.totVertexes - 2 + last.circ.totVertexes - 2;
m = new UncheckedMesh(nVrt, nTri);
first.circ.WriteEndcap(first.dia, first.y, false, 0, 0, m, false);
last.circ.WriteEndcap(last.dia, last.y, true, first.circ.totVertexes, (first.circ.totVertexes - 2) * 3, m, !odd);
m.WriteTo(EndsMesh);
// build the collider mesh at a lower resolution than the visual mesh.
if (customCollider)
{
//Debug.LogWarning("Collider mesh vert=" + nColVrt + " tris=" + nColTri);
m = new UncheckedMesh(nColVrt, nColTri);
odd = false;
{
ProfilePoint prev = null;
int off = 0, prevOff = 0;
int tOff = 0;
foreach (ProfilePoint pt in pts)
{
if (!pt.inCollider)
{
continue;
}
//Debug.LogWarning("Collider circ (" + pt.dia + ", " + pt.y + ") verts=" + pt.colliderCirc.totVertexes);
pt.colliderCirc.WriteVertexes(diameter: pt.dia, y: pt.y, v: pt.v, norm: pt.norm, off: off, m: m, odd: odd);
if (prev != null)
{
CirclePoints.WriteTriangles(prev.colliderCirc, prevOff, pt.colliderCirc, off, m.triangles, tOff * 3, !odd);
tOff += prev.colliderCirc.totVertexes + pt.colliderCirc.totVertexes;
}
prev = pt;
prevOff = off;
off += pt.colliderCirc.totVertexes + 1;
odd = !odd;
}
}
if (colliderMesh == null)
{
colliderMesh = new Mesh();
}
m.WriteTo(colliderMesh);
PPart.ColliderMesh = colliderMesh;
}
else
{
PPart.ColliderMesh = SidesMesh;
}
RaiseModelAndColliderChanged();
}
private static Quaternion SideAttachOrientation(ProfilePoint pv, ProfilePoint pt, float theta, out Vector3 normal)
{
normal = Quaternion.AngleAxis(theta * 180 / Mathf.PI, Vector3.up) * new Vector2(pt.y - pv.y, -(pt.dia - pv.dia) / 2f);
return(Quaternion.FromToRotation(Vector3.up, normal));
}
private void MoveAttachments(LinkedList <ProfilePoint> pts)
{
lastProfile = pts;
// top points
ProfilePoint top = pts.Last.Value;
foreach (Attachment a in topAttachments)
{
Vector3 pos = new Vector3(
(a.uv[0] - 0.5f) * top.dia * 0.5f,
top.y,
(a.uv[1] - 0.5f) * top.dia * 0.5f);
//Debug.LogWarning("Moving attachment:" + a + " to:" + pos.ToString("F7") + " uv: " + a.uv.ToString("F5"));
a.follower.transform.localPosition = pos;
a.follower.Update();
}
// bottom points
ProfilePoint bot = pts.First.Value;
foreach (Attachment a in bottomAttachments)
{
Vector3 pos = new Vector3(
(a.uv[0] - 0.5f) * bot.dia * 0.5f,
bot.y,
(a.uv[1] - 0.5f) * bot.dia * 0.5f);
//Debug.LogWarning("Moving attachment:" + a + " to:" + pos.ToString("F7") + " uv: " + a.uv.ToString("F5"));
a.follower.transform.localPosition = pos;
a.follower.Update();
}
// sides
ProfilePoint pv = null;
ProfilePoint pt = pts.First.Value;
LinkedListNode <ProfilePoint> ptNode = pts.First;
foreach (Attachment a in sideAttachments)
{
while (pt.v < a.uv[1])
{
ptNode = ptNode.Next;
if (ptNode == null)
{
ptNode = pts.Last;
Debug.LogError("Child v greater than last point. Child v=" + a.uv[1] + " last point v=" + ptNode.Value.v);
break;
}
if (!ptNode.Value.inCollider)
{
continue;
}
pv = pt;
pt = ptNode.Value;
}
if (pv == null)
{
Debug.LogError("Child v smaller than first point. Child v=" + a.uv[1] + " first point v=" + ptNode.Value.v);
continue;
}
float t = Mathf.InverseLerp(pv.v, pt.v, a.uv[1]);
//Debug.LogWarning("pv.v=" + pv.v + " pt.v=" + pt.v + " att.v=" + a.uv[1] + " t=" + t);
// using cylindrical coords
float r = Mathf.Lerp(pv.dia * 0.5f, pt.dia * 0.5f, t);
float y = Mathf.Lerp(pv.y, pt.y, t);
float theta = Mathf.Lerp(0, Mathf.PI * 2f, a.uv[0]);
float x = Mathf.Cos(theta) * r;
float z = -Mathf.Sin(theta) * r;
Vector3 pos = new Vector3(x, y, z);
//print("Moving attachment:" + a + " to:" + pos.ToString("F3"));
a.follower.transform.localPosition = pos;
Vector3 normal;
Quaternion rot = SideAttachOrientation(pv, pt, theta, out normal);
//Debug.LogWarning("Moving to orientation: normal: " + normal.ToString("F3") + " theta:" + (theta * 180f / Mathf.PI) + rot.ToStringAngleAxis());
a.follower.transform.localRotation = rot;
a.follower.Update();
}
}
public static void Reset()
{
profiles.Clear();
startTime = DateTime.UtcNow;
if (fastProfiles != null) {
for (int i=0;i<fastProfiles.Length;i++) {
fastProfiles[i] = new ProfilePoint ();
}
}
}
private void WriteShape()
{
LinkedList <ProfilePoint> points = new LinkedList <ProfilePoint>();
int colliderTri = 0;
points.AddLast(CreatePoint(0, ref colliderTri));
points.AddLast(CreatePoint(1, ref colliderTri));
colliderTri /= 2;
Queue <LinkedListNode <ProfilePoint> > process = new Queue <LinkedListNode <ProfilePoint> >();
process.Enqueue(points.First);
while (process.Count > 0)
{
LinkedListNode <ProfilePoint> node = process.Dequeue();
ProfilePoint pM = node.Value;
// ReSharper disable once PossibleNullReferenceException
ProfilePoint pN = node.Next.Value;
float tM = pM.v;
float tN = pN.v;
// So we want to find the point where the curve is maximally distant from the line between pM and pN
// First we need the normal to the line:
Vector2 norm = new Vector2(-pN.y + pM.y, pN.dia - pM.dia);
// The deviation is:
// Dev = B(t) . norm - B(m) . norm (where m = t at point M)
// We want to know the maxima, so take the derivative and solve for = 0
// Dev' = B'(t) . norm
// = 3(1-t)^2 ((p1.x-p0.x) norm.x + (p1.y-p0.y) norm.y) + 6t(1-t) ((p2.x-p1.x) norm.x + (p2.y-p1.y) norm.y) + 3t^2 ((p3.x-p2.x) norm.x + (p3.y-p2.y) norm.y) = 0
// This is a quadratic, which we can solve directly.
float a = ((p1.x - p0.x) * norm.x + (p1.y - p0.y) * norm.y);
float b = ((p2.x - p1.x) * norm.x + (p2.y - p1.y) * norm.y);
float c = ((p3.x - p2.x) * norm.x + (p3.y - p2.y) * norm.y);
// solve a (1-t)^2+2 b (t (1-t))+c t^2 = 0
// t = (-/+ sqrt(b^2-a c)-a+b)/(-a+2 b-c) for a-2 b+c!=0
// t = (2 b-c)/(2 (b-c)) for a = 2 b-c and b-c!=0
List <float> ts = new List <float>(2);
//Debug.LogWarning(string.Format("t={0:F3}..{1:F3} perp=({2:F3}, {3:F3}) a={4:F3} b={5:F3} c={6:F3}", tM, tN, norm.x, norm.y, a, b, c));
if (Math.Abs(a - 2 * b + c) < 1e-6f)
{
if (Math.Abs(b - c) < 1e-6f)
{
// This is the straight line case, no need to subdivide
continue;
}
float t1 = (2f * b - c) / (2f * (b - c));
//Debug.LogWarning(string.Format("t={0:F3}..{1:F3} -> {2:F3}", tM, tN, t1));
ts.Add(t1);
}
else
{
float sqrt = Mathf.Sqrt(b * b - a * c);
float t1 = (sqrt - a + b) / (-a + 2 * b - c);
float t2 = (-sqrt - a + b) / (-a + 2 * b - c);
//Debug.LogWarning(string.Format("t={0:F3}..{1:F3} -> {2:F3} {3:F3} ", tM, tN, t1, t2));
ts.Add(t1);
ts.Add(t2);
ts.Sort();
}
for (int i = 0; i < ts.Count; ++i)
{
if (ts[i] < tM || ts[i] > tN)
{
ts.RemoveAt(i--);
}
}
if (ts.Count == 0)
{
throw new InvalidProgramException("There should be a point maximally distant from the line or the maths is really wrong.");
}
norm = norm.normalized;
float devM = pM.dia * norm.x + pM.y * norm.y;
for (int i = 0; i < ts.Count; ++i)
{
// The difference from the line
float devTS = Vector2.Dot(B(ts[i]), norm) - devM;
if (Mathf.Abs(devTS) < MaxCircleError)
{
ts.RemoveAt(i--);
}
}
switch (ts.Count)
{
case 0:
break;
case 1:
LinkedListNode <ProfilePoint> next = node.List.AddAfter(node, CreatePoint(ts[0], ref colliderTri));
process.Enqueue(node);
process.Enqueue(next);
break;
case 2:
LinkedListNode <ProfilePoint> next0 = node.List.AddAfter(node, CreatePoint(ts[0], ref colliderTri));
LinkedListNode <ProfilePoint> next1 = node.List.AddAfter(next0, CreatePoint(ts[1], ref colliderTri));
process.Enqueue(node);
process.Enqueue(next0);
process.Enqueue(next1);
break;
}
}
// Need to figure out the v coords.
float sumLengths = 0;
float[] cumLengths = new float[points.Count - 1];
LinkedListNode <ProfilePoint> pv = points.First;
LinkedListNode <ProfilePoint> nx = pv.Next;
for (int i = 0; i < cumLengths.Length; ++i, pv = nx, nx = nx.Next)
{
// ReSharper disable once PossibleNullReferenceException
float dX = nx.Value.dia - pv.Value.dia;
float dY = nx.Value.y - pv.Value.y;
cumLengths[i] = sumLengths += Mathf.Sqrt(dX * dX + dY * dY);
}
points.First.Value.v = 0;
nx = points.First.Next;
for (int i = 0; i < cumLengths.Length; ++i, nx = nx.Next)
{
// ReSharper disable once PossibleNullReferenceException
nx.Value.v = cumLengths[i] / sumLengths;
}
WriteMeshes(points);
}
private void UpdateNodeSize(ProfilePoint pt, string nodeName)
{
AttachNode node = part.attachNodes.Find(n => n.id == nodeName);
if (node == null)
return;
node.size = Math.Min((int)(pt.dia / pPart.diameterLargeStep), 3);
// Breaking force and torque scales with the area of the surface (node size).
node.breakingTorque = node.breakingForce = Mathf.Max(50 * node.size * node.size, 50);
// Send messages for the changing of the ends
RaiseChangeAttachNodeSize(nodeName, pt.dia, Mathf.PI * pt.dia * pt.dia * 0.25f, node.size);
// TODO: separate out the meshes for each end so we can use the scale for texturing.
RaiseChangeTextureScale(nodeName, pPart.endsMaterial, new Vector2(pt.dia, pt.dia));
}
