public override void updateMesh(AnimatedMeshState state) { Vector3[] vertices = state.mesh.vertices; AnimatedConeState acState = state.data as AnimatedConeState; ConeState current = acState.currentState; float lifeRatio = state.lifeRatio; int numVertices = this.data.numVertices; int i; current.length = Mathf.Lerp(this.data.initialState.length, this.data.finalState.length, this.data.interpolationCurve.Evaluate(lifeRatio)); current.radiusBottom = Mathf.Lerp(this.data.initialState.radiusBottom, this.data.finalState.radiusBottom, this.data.interpolationCurve.Evaluate(lifeRatio)); current.radiusTop = Mathf.Lerp(this.data.initialState.radiusTop, this.data.finalState.radiusTop, this.data.interpolationCurve.Evaluate(lifeRatio)); for (i = 0; i < numVertices; ++i) { float angle = 2 * Mathf.PI * i / numVertices; float angleSin = Mathf.Sin(angle); float angleCos = Mathf.Cos(angle); vertices[i] = new Vector3(current.radiusTop * angleCos, current.radiusTop * angleSin, -current.length); vertices[i + numVertices + 1] = new Vector3(current.radiusBottom * angleCos, current.radiusBottom * angleSin, 0); } // connecting edges ++numVertices; vertices[i] = new Vector3(vertices[0].x, vertices[0].y, vertices[0].z); vertices[i + numVertices] = new Vector3(vertices[numVertices].x, vertices[numVertices].y, vertices[numVertices].z); --numVertices; state.mesh.vertices = vertices; if (this.data.materialModifier && acState.mr) { this.data.materialModifier.update(acState.materialModifierState, this.data.interpolationCurve.Evaluate(lifeRatio)); this.data.materialModifier.updateMaterial(acState.materialModifierState, acState.mr.material); } }
public override Mesh generateMesh(AnimatedMeshState state) { AnimatedConeState acState = state.data as AnimatedConeState; Vector3[] vertices; Mesh mesh; int numVertices = this.data.numVertices; ConeState init = this.data.initialState; string path = meshPrefabPath + "Cone" + this.data.numVertices + "v" + init.radiusTop + "t" + init.radiusBottom + "b" + init.length + "l" + init.length + "o" + ".asset"; if (!this.createAsset || !(mesh = (Mesh)AssetDatabase.LoadAssetAtPath(path, typeof(Mesh)))) { mesh = new Mesh(); mesh.Clear(); numVertices += 1; // for correct uv mapping (to join correctly the faces) vertices = new Vector3[2 * numVertices]; // 0..n-1: top, n..2n-1: bottom Vector3[] normals = new Vector3[2 * numVertices]; Vector2[] uvs = new Vector2[2 * numVertices]; int[] tris; float slope = Mathf.Atan((init.radiusBottom - init.radiusTop) / init.length); // (rad difference)/height float slopeSin = Mathf.Sin(slope); float slopeCos = Mathf.Cos(slope); int i; --numVertices; for (i = 0; i < numVertices; ++i) { float angle = 2 * Mathf.PI * i / numVertices; float angleSin = Mathf.Sin(angle); float angleCos = Mathf.Cos(angle); float angleHalf = 2 * Mathf.PI * (i + 0.5f) / numVertices; // for degenerated normals at cone tips float angleHalfSin = Mathf.Sin(angleHalf); float angleHalfCos = Mathf.Cos(angleHalf); vertices[i] = new Vector3(init.radiusTop * angleCos, init.radiusTop * angleSin, -init.length); vertices[i + numVertices + 1] = new Vector3(init.radiusBottom * angleCos, init.radiusBottom * angleSin, 0); normals[i] = new Vector3(angleCos * slopeCos, angleSin * slopeCos, -slopeSin); normals[i + numVertices + 1] = new Vector3(angleCos * slopeCos, angleSin * slopeCos, -slopeSin); uvs[i] = new Vector2(1.0f * i / numVertices, 1); uvs[i + numVertices + 1] = new Vector2(1.0f * i / numVertices, 0); } // connecting edges ++numVertices; vertices[i] = new Vector3(vertices[0].x, vertices[0].y, vertices[0].z); vertices[i + numVertices] = new Vector3(vertices[numVertices].x, vertices[numVertices].y, vertices[numVertices].z); uvs[i] = new Vector2(1, 1); uvs[i + numVertices] = new Vector2(1, 0); normals[i] = new Vector3(normals[0].x, normals[0].y, normals[0].z); normals[i + numVertices] = new Vector3(normals[numVertices].x, normals[numVertices].y, normals[numVertices].z); mesh.vertices = vertices; mesh.normals = normals; mesh.uv = uvs; // creating triangles int cnt = 0; tris = new int[numVertices * 6]; for (i = 0; i < numVertices; i++) { int ip1 = i + 1; if (ip1 == numVertices) { ip1 = 0; } tris[cnt++] = i; tris[cnt++] = ip1; tris[cnt++] = i + numVertices; tris[cnt++] = ip1 + numVertices; tris[cnt++] = i + numVertices; tris[cnt++] = ip1; } --numVertices; mesh.triangles = tris; if (this.createAsset) { AssetDatabase.CreateAsset(mesh, path); AssetDatabase.SaveAssets(); } } return(mesh); }
public void copy(ConeState other) { this.radiusBottom = other.radiusBottom; this.radiusTop = other.radiusTop; this.length = other.length; }
void PixyMqRecvd(object sender, MqttMsgPublishEventArgs e) { // xCon.t("."); if (SubState == VisionState.Run) { dynamic a = JsonConvert.DeserializeObject(Encoding.UTF8.GetString(e.Message)); DateTime nowTime = DateTime.Now; // lots or no rects is a good indication we dont have the cone // after some elapsed time of no cone, return servo to 90 if (1.0 / (int)a["Count"] < .49) switch (coneFlag) // no cone { case ConeState.Found: coneFlag = ConeState.LostWait; break; } else { lastSeenTime = nowTime; switch (coneFlag) // cone { case ConeState.Lost: FoundCone(); break; case ConeState.LostWait: // todo code smell coneFlag = ConeState.Found; goto case ConeState.Found; case ConeState.Found: float et = (float)((nowTime - pidCycleTime).Milliseconds) / 1000; if (et == 0 || a == null) break; // 160 should be center, but we adjust as per reality servoPosition -= (int)Pid(160F, (float)(a.Center), kP, kI, kD, ref prevErr, ref integral, ref derivative, et, 1); //Trace.t(cc.Norm, string.Format("Steering srvoPosition {0} e({1:F2}) i({2}) d({3}) et({4})", servoPosition, prevErr, integral, derivative, et)); if (servoPosition < 0) servoPosition = 0; else if (servoPosition > 180) servoPosition = 180; Program.Pilot.Send(new { Cmd = "SRVO", Value = servoPosition }); break; } pidCycleTime = nowTime; } } }
private void LostCone() { coneFlag = ConeState.Lost; Trace.t(cc.Warn, "Vision::Cone lost"); Console.Beep(); servoPosition = 90; Program.Pilot.Send(new { Cmd = "SRVO", Value = 90 }); if (OnLostCone != null) OnLostCone(this, null); }
private void FoundCone() { coneFlag = ConeState.Found; Trace.t(cc.Good, "Vision::Cone Found"); Console.Beep(); prevErr = integral = derivative = 0; if (OnFoundCone != null) OnFoundCone(this, null); }