void Scale2VR() { cameraPosition = vrCamera.transform.position; float scaleBy = cameraPosition.y / head.position.y; this.transform.parent.localScale = new Vector3(this.transform.parent.localScale.x * scaleBy, this.transform.parent.localScale.y * scaleBy, this.transform.parent.localScale.z * scaleBy); //this.transform.parent.position = new Vector3(cameraPosition.x - head.position.x, cameraPosition.y / 2 - head.position.y, cameraPosition.z - head.position.x); //this.transform.parent.rot //Get the latest Tranform positions skeletonPoints[0] = head.position; skeletonPoints[1] = rightHand.position; skeletonPoints[2] = leftHand.position; vrPoints[0] = new Vector4(vrCamera.transform.position.x, vrCamera.transform.position.y, vrCamera.transform.position.z, vrCamera.transform.localScale.x); vrPoints[1] = new Vector4(vrRightHand.transform.position.x, vrRightHand.transform.position.y, vrRightHand.transform.position.z, vrRightHand.transform.localScale.x); vrPoints[2] = new Vector4(vrLeftHand.transform.position.x, vrLeftHand.transform.position.y, vrLeftHand.transform.position.z, vrLeftHand.transform.localScale.x); Matrix4x4 kabschTransform = solver.SolveKabsch(skeletonPoints, vrPoints); head.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[0]); rightHand.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[1]); leftHand.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[2]); scalingComplete = true; }
//This function is called when align buttion is clicked. public void AlignModel() { Debug.Log("Align Model"); benchtopSharing.IsManipulated = true; virtualLandmarkPositions = GetAllLandmarkPositions(virtualLandmarks); for (int i = 0; i < virtualLandmarks.Length; i++) { refPoints[i] = new Vector4(realLandmarkPositions[i].x, realLandmarkPositions[i].y, realLandmarkPositions[i].z, 1); Debug.Log(refPoints[i]); } Matrix4x4 kabschTranform = solver.SolveKabsch(virtualLandmarkPositions, refPoints); Vector3 beforeCentroid = AlignmentHelper.GetCentroidPosition(virtualLandmarkPositions); Debug.Log("Before Centroid" + beforeCentroid); for (int i = 0; i < virtualLandmarks.Length; i++) { virtualLandmarkPositions[i] = kabschTranform.MultiplyPoint3x4(virtualLandmarkPositions[i]); } Vector3 afterCentroid = AlignmentHelper.GetCentroidPosition(virtualLandmarkPositions); Debug.Log("After Centroid" + beforeCentroid); // translate model model.position += afterCentroid - beforeCentroid; // rotate model Quaternion rotation = kabschTranform.GetQuaternion(); model.RotateAroundPivot(afterCentroid, rotation); }
void Update() { solver = new KabschSolver(); for (int iteration = 0; iteration < 2; iteration++) { float currentCost = 0f; for (int i = 0; i < features.Length; i++) { refPoints[i] = features[i].position; inPoints[i] = Constraints.ConstrainToSegment(features[i].position, aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i] * 6f)); currentCost += Vector3.Distance(refPoints[i], inPoints[i]); Debug.DrawLine(aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i])); Debug.DrawLine(features[i].position, inPoints[i], Color.red); } Matrix4x4 iterationStep = solver.SolveKabsch(inPoints, refPoints, true); Matrix4x4 steppedTransform = iterationStep * aligningCamera.localToWorldMatrix; if (useLinearEstimate) { float stepScale = ((previousCost - currentCost) > 0.0000001f && iteration > 0) ? (previousCost / (previousCost - currentCost)) * 0.8f : 1f; aligningCamera.position += (steppedTransform.GetVector3() - aligningCamera.position) * stepScale; aligningCamera.rotation = Quaternion.SlerpUnclamped(Quaternion.identity, iterationStep.GetQuaternion(), stepScale) * aligningCamera.rotation; } else { aligningCamera.position = steppedTransform.GetVector3(); aligningCamera.rotation = steppedTransform.GetQuaternion(); } previousCost = currentCost; } }
void Update() { for (int iteration = 0; iteration < 1; iteration++) { for (int i = 0; i < features.Length; i++) { Debug.DrawLine(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i])); Debug.DrawLine(cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i])); float timeLineOne, timeLineTwo; line2lineDisplacement(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]), out timeLineOne, out timeLineTwo); //Take the abs of the times so they can't intersect behind the camera timeLineOne = Mathf.Abs(timeLineOne); timeLineTwo = Mathf.Abs(timeLineTwo); inPoints[i] = Vector3.LerpUnclamped(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), timeLineOne); refPoints[i] = Vector3.LerpUnclamped(cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]), timeLineTwo); Debug.DrawLine(inPoints[i], refPoints[i], Color.red); } Matrix4x4 iterationStep = solver.SolveKabsch(refPoints, inPoints, true); cameras[1].position += iterationStep.GetVector3(); cameras[1].rotation = iterationStep.GetQuaternion() * cameras[1].rotation; } }
void Update() { for (int cameraIndex = 0; cameraIndex < 4; cameraIndex++) { for (int i = 0; i < features.Length; i++) { Debug.DrawLine(cameras[cameraIndex].position, cameras[cameraIndex].TransformPoint(rayDirections[cameraIndex][i])); } } Profiler.BeginSample("Bundle Adjustment"); for (int iteration = 0; iteration < 1; iteration++) { for (int i = 0; i < features.Length; i++) { for (int cameraIndex = 0; cameraIndex < 4; cameraIndex++) { Vector3 pointLineOne, pointLineTwo; Displacement(cameras[(cameraIndex / 2)].position, cameras[(cameraIndex / 2)].TransformPoint(rayDirections[(cameraIndex / 2)][i]), cameras[(cameraIndex % 2) + 2].position, cameras[(cameraIndex % 2) + 2].TransformPoint(rayDirections[(cameraIndex % 2) + 2][i]), out pointLineOne, out pointLineTwo); inPoints [(i * cameras.Length) + cameraIndex] = pointLineOne; refPoints[(i * cameras.Length) + cameraIndex] = pointLineTwo; Debug.DrawLine(pointLineOne, pointLineTwo, Color.red); } } Matrix4x4 iterationStep = solver.SolveKabsch(refPoints, inPoints, true); cameras[2].parent.position += iterationStep.GetVector3(); cameras[2].parent.rotation = iterationStep.GetQuaternion() * cameras[2].parent.rotation; } Profiler.EndSample(); }
void Update() { //Translate the points into world space for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.TransformPoint(bodyVerts[i]); } for (int i = 0; i < solverIterations; i++) { //First, ensure that the surface area is what we think it is Verlet.resolveDistanceConstraints(constraints, ref bodyVerts, ref accumulatedDisplacements, 1); //Next, set the volume of the soft body Verlet.setVolume(inflationAmount * initialVolume, bodyVerts, bodyNormals, bodyTriangles, initialSurfaceArea, true, fastButGarbage); } //Also clamp to the intersection of capsules for shits for (int j = 0; j < bodyVerts.Length; j++) { Vector3 capOne = Constraints.ConstrainToCapsule(bodyVerts[j], Vector3.zero, Vector3.up, 0.25f) - bodyVerts[j]; Vector3 capTwo = Constraints.ConstrainToCapsule(bodyVerts[j], Vector3.zero, Vector3.right * 1.15f, 0.25f) - bodyVerts[j]; if (capOne.sqrMagnitude < capTwo.sqrMagnitude) { bodyVerts[j] += capOne; } else { bodyVerts[j] += capTwo; } } //Calculate the the position and rotation of the body for (int i = 0; i < bodyVerts.Length; i++) { kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f); } ; Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, Array.ConvertAll(bodyVerts, (p => (Vector4)p))); transform.position = toWorldSpace.GetVector3(); transform.rotation = toWorldSpace.GetQuaternion(); //Move the points into local space for rendering for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]); renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]); } Debug.Log(Verlet.VolumeOfMesh(bodyVerts, bodyTriangles)); //Graphics bodyMesh.vertices = bodyVerts; bodyMesh.normals = renderNormals; bodyMesh.RecalculateBounds(); bodyMesh.UploadMeshData(false); }
public static Quaternion GetRotation(List <Vector3> a, List <Vector3> b) { var closest = ComputePointMapping(a, b); var c = (from i in closest.Values select b[i]).ToList(); KabschSolver solver = new KabschSolver(); var matrix = solver.SolveKabsch(a, c); return(matrix.GetQuaternion()); }
//Calculate the Kabsch Transform and Apply it to the input points void Update() { for (int i = 0; i < inPoints.Length; i++) { refPoints[i] = new Vector4(referencePoints[i].position.x, referencePoints[i].position.y, referencePoints[i].position.z, referencePoints[i].localScale.x); } Matrix4x4 kabschTransform = solver.SolveKabsch(points, refPoints); for (int i = 0; i < inPoints.Length; i++) { inPoints[i].position = kabschTransform.MultiplyPoint3x4(points[i]); } }
//Calculate the Kabsch Transform and Apply it to the input points void Update() { for (int i = 0; i < inPoints.Length; i++) { refPoints[i] = referencePoints[i].position; } Matrix4x4 kabschTransform = solver.SolveKabsch(points, refPoints); for (int i = 0; i < inPoints.Length; i++) { inPoints[i].position = kabschTransform.MultiplyPoint3x4(points[i]); } }
void Update() { //Translate the points into world space for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.TransformPoint(bodyVerts[i]); } //Physics float currentDeltaTime = Mathf.Clamp(Time.deltaTime, 0.01f, previousDeltaTime * 1.4f); Verlet.Integrate(bodyVerts, prevBodyVerts, scaledGravity, currentDeltaTime, previousDeltaTime); previousDeltaTime = currentDeltaTime; //Anchor a point on the body if (anchor != null && anchor.gameObject.activeInHierarchy) { bodyVerts[0] = prevBodyVerts[0] = anchor.position; } //Also sneak in a ground plane here: Vector3 groundPlanePos = groundPlane.position; Vector3 groundPlaneNormal = -groundPlane.forward; for (int j = 0; j < bodyVerts.Length; j++) { if (Vector3.Dot(bodyVerts[j] - groundPlanePos, groundPlaneNormal) < 0f) { bodyVerts[j] = Vector3.ProjectOnPlane(bodyVerts[j] - groundPlanePos, groundPlaneNormal) + groundPlanePos; bodyVerts[j] -= Vector3.ProjectOnPlane(bodyVerts[j] - prevBodyVerts[j], groundPlaneNormal) * 0.3f; } } //Calculate the the position and rotation of the body for (int i = 0; i < bodyVerts.Length; i++) { kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f); } ; kabschVerts.CopyTo(kabschVertsArray); Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVertsArray, true); constrainVertsToDeformation(originalVerts, toWorldSpace, ref bodyVerts); Verlet.RecalculateNormalsNonAlloc(bodyVerts, bodyTriangles, ref bodyNormals); //Move the points into local space for rendering transform.position = toWorldSpace.GetVector3(); transform.rotation = toWorldSpace.GetQuaternion(); for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]); renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]); } Debug.DrawRay(transform.position, toWorldSpace * (Vector3.Cross(yBasis, zBasis).normalized *xScale), Color.red); Debug.DrawRay(transform.position, toWorldSpace * yBasis, Color.green); Debug.DrawRay(transform.position, toWorldSpace * zBasis, Color.blue); //Graphics bodyVerts.CopyTo(bodyVertsArray); renderNormals.CopyTo(renderNormalsArray); bodyMesh.vertices = bodyVertsArray; bodyMesh.normals = renderNormalsArray; bodyMesh.RecalculateBounds(); bodyMesh.UploadMeshData(false); }
void Update() { //Translate the points into world space for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.TransformPoint(bodyVerts[i]); } //Physics float currentDeltaTime = Mathf.Clamp(Time.deltaTime, 0.01f, previousDeltaTime * 1.4f); Verlet.Integrate(bodyVerts, prevBodyVerts, scaledGravity, currentDeltaTime, previousDeltaTime); previousDeltaTime = currentDeltaTime; //Anchor a point on the body if (anchor != null && anchor.gameObject.activeInHierarchy) { bodyVerts[0] = prevBodyVerts[0] = anchor.position; } for (int i = 0; i < solverIterations; i++) { //First, ensure that the surface area is what we think it is Verlet.resolveDistanceConstraints(constraints, ref bodyVerts, ref accumulatedDisplacements, 1); //Next, set the volume of the soft body Verlet.setVolume(inflationAmount * initialVolume, bodyVerts, bodyNormals, bodyTriangles, initialSurfaceArea, true, fastButGarbage); } //Also sneak in a ground plane here: Vector3 groundPlanePos = groundPlane.position; Vector3 groundPlaneNormal = -groundPlane.forward; for (int j = 0; j < bodyVerts.Length; j++) { if (Vector3.Dot(bodyVerts[j] - groundPlanePos, groundPlaneNormal) < 0f) { bodyVerts[j] = Vector3.ProjectOnPlane(bodyVerts[j] - groundPlanePos, groundPlaneNormal) + groundPlanePos; bodyVerts[j] -= Vector3.ProjectOnPlane(bodyVerts[j] - prevBodyVerts[j], groundPlaneNormal) * 0.3f; } } //Calculate the the position and rotation of the body for (int i = 0; i < bodyVerts.Length; i++) { kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f); } ; Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVerts, transformFollowsRotation); transform.position = toWorldSpace.GetVector3(); transform.rotation = toWorldSpace.GetQuaternion(); //Move the points into local space for rendering for (int i = 0; i < bodyVerts.Length; i++) { bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]); renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]); } //Graphics bodyMesh.vertices = bodyVerts; bodyMesh.normals = renderNormals; bodyMesh.RecalculateBounds(); bodyMesh.UploadMeshData(false); }
private void Update() { //Update the kinect data for (int i = 0; i < users.Length; i++) { if (users[i].skeletonProvider != null) { KinectSkeleton skeleton = users[i].GetSkeleton(); if (skeleton != null && skeleton.valid) { if (!users[i].created) { users[i].skeletonProvider.CreateSkeleton(users[i]); users[i].created = true; } if (users[i].updateFromKinect) { /*transform.position = getHeadPosition(); * if (lookAt != null) * { * transform.LookAt(lookAt.transform.position); * } * //else transform.localRotation = Quaternion.identity;*/ users[i].skeletonProvider.RefreshBodyObject(users[i]); } } } } #region Kabsch implementation if (kabsch) { //Get the latest Tranform positions for (int i = 0; i < users.Length; i++) { if (users[i].GetSkeleton() != null && users[i].GetSkeleton().valid) { for (int j = 0; j < users[i].skeletonProvider.joints.Length; j++) { pointSets[i][j] = users[i].skeletonProvider.joints[j].position; } } } for (int j = 0; j < targetPointSet.Length; j++) { if (users[0].GetSkeleton() != null && users[0].GetSkeleton().valid) { targetPointSet[j] = new Vector4(users[0].skeletonProvider.joints[j].position.x, users[0].skeletonProvider.joints[j].position.y, users[0].skeletonProvider.joints[j].position.z, users[0].skeletonProvider.joints[j].localScale.x); } } //Calculate new positions for the skeletons for (int k = 1; k < users.Length; k++) { if (users[k].GetSkeleton() != null && users[k].GetSkeleton().valid) { Matrix4x4 kabschTransform = solver.SolveKabsch(pointSets[k], targetPointSet); for (int i = 0; i < users[k].skeletonProvider.joints.Length; i++) { users[k].skeletonProvider.joints[i].position = kabschTransform.MultiplyPoint3x4(pointSets[k][i]); for (int j = 0; j < HumanTopology.BONE_CONNECTIONS.Length; j++) { BoneConnection bone = HumanTopology.BONE_CONNECTIONS[j]; JointType joint1 = bone.fromJoint; JointType joint2 = bone.toJoint; LineRenderer lr = users[k].skeletonProvider.joints[(int)joint2].GetComponent <LineRenderer>(); lr.SetPosition(0, users[k].skeletonProvider.joints[(int)joint1].position); lr.SetPosition(1, users[k].skeletonProvider.joints[(int)joint2].position); } /*for (int j = 0; j < HumanTopology.BONE_CONNECTIONS.Length; j++) * { * BoneConnection bone = HumanTopology.BONE_CONNECTIONS[j]; * JointType joint1 = bone.fromJoint; * JointType joint2 = bone.toJoint; * KinectSkeleton.TrackingState state1 = users[k].GetSkeleton().jointStates[(int)joint1]; * KinectSkeleton.TrackingState state2 = users[k].GetSkeleton().jointStates[(int)joint2]; * LineRenderer lr = users[k].skeletonProvider.joints[i].GetComponent<LineRenderer>(); * bool tracked = state1 != KinectSkeleton.TrackingState.NotTracked && state2 != KinectSkeleton.TrackingState.NotTracked; * if (tracked) * { * lr.SetPosition(0, users[k].skeletonProvider.joints[(int)joint1].position); * lr.SetPosition(1, users[k].skeletonProvider.joints[(int)joint2].position); * } * }*/ } } } } #endregion #region Kalman implementation if (kalman) { for (int i = 0; i < HumanTopology.JOINT_NAMES.Length; i++) { for (int j = 0; j < users.Length; j++) { if (users[j].GetSkeleton() != null && users[j].GetSkeleton().valid) { filters[i].PredictState(Time.deltaTime); filters[i].UpdateState(new double[, ] { { users[j].skeletonProvider.joints[i].position.x }, { users[j].skeletonProvider.joints[i].position.y }, { users[j].skeletonProvider.joints[i].position.z }, { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 } }); double oldPos = filters[i].StateMatrix[0, 0]; /*for (int k = 1; k < 10; k++) * { * double[,] PredictedState = filters[i].SafePredictState(0.1 * k); * Debug.DrawLine(new Vector3((float)oldPos, k * 0.02f, 0f), new Vector3((float)PredictedState[0, 0], (k + 1) * 0.02f, 0f), Color.green); * oldPos = PredictedState[0, 0]; * * Debug.DrawLine(new Vector3(Mathf.Sin((Time.time + (k * 0.1f))), k * 0.02f, 0f), * new Vector3(Mathf.Sin((Time.time + ((k + 1f) * 0.1f))), (k + 1) * 0.02f, 0f), Color.red); * }*/ fusedJoints[i].position = new Vector3((float)filters[i].StateMatrix[0, 0], (float)filters[i].StateMatrix[1, 0], (float)filters[i].StateMatrix[2, 0]); } } } for (int k = 0; k < HumanTopology.BONE_CONNECTIONS.Length; k++) { BoneConnection bone = HumanTopology.BONE_CONNECTIONS[k]; JointType joint1 = bone.fromJoint; JointType joint2 = bone.toJoint; LineRenderer lr = fusedJoints[(int)joint2].GetComponent <LineRenderer>(); lr.SetPosition(0, fusedJoints[(int)joint1].position); lr.SetPosition(1, fusedJoints[(int)joint2].position); } readyToScale = true; } #endregion }
// Update is called once per frame void Update() { if (devices.Length > 1) { // Add the set of joints to device-specific lists if (Input.GetKeyUp(takeCalibrationSampleKey)) { bool handInAllFrames = true; for (int i = 0; i < devices.Length; i++) { Hand rightHand = devices[i].deviceProvider.CurrentFrame.Get(Chirality.Right); if (rightHand != null) { devices[i].currentHand = rightHand; } else { handInAllFrames = false; } } if (handInAllFrames) { for (int i = 0; i < devices.Length; i++) { if (devices[i].handPoints == null) { devices[i].handPoints = new List <Vector3>(); } for (int j = 0; j < 5; j++) { for (int k = 0; k < 4; k++) { devices[i].handPoints.Add(devices[i].currentHand.Fingers[j].bones[k].Center.ToVector3()); } } } } } // Moves subsidiary devices to be in alignment with the device at the 0 index if (Input.GetKeyUp(solveForRelativeTransformKey)) { if (devices[0].handPoints.Count > 3) { for (int i = 1; i < devices.Length; i++) { KabschSolver solver = new KabschSolver(); Matrix4x4 deviceToOriginDeviceMatrix = solver.SolveKabsch(devices[i].handPoints, devices[0].handPoints, 200); devices[i].deviceProvider.transform.Transform(deviceToOriginDeviceMatrix); devices[i].handPoints.Clear(); } devices[0].handPoints.Clear(); } } } }
void Update() { Profiler.BeginSample("Schedule Softbody Work", this); //Transform the points into world space JobHandle localToWorldHandle = new ToWorldSpaceJob() { localToWorld = transform.localToWorldMatrix, bodyVerts = softbodyData.bodyVerts }.Schedule(originalVerts.Length, 64); //Physics - Verlet Integration JobHandle verletHandle = new VerletIntegrateJob() { bodyVerts = softbodyData.bodyVerts, prevBodyVerts = softbodyData.prevBodyVerts, scaledGravity = softbodyData.scaledGravity }.Schedule(originalVerts.Length, 64, dependsOn: localToWorldHandle); JobHandle previousHandle = verletHandle; for (int i = 0; i < solverIterations; i++) { //First, ensure that the surface area is what we think it is JobHandle accumulateDistances = new AccumulateDistanceConstraintsJob() { bodyVerts = softbodyData.bodyVerts, accumulatedDisplacements = softbodyData.accumulatedDisplacements, constraintsArray = softbodyData.constraintsArray }.Schedule(dependsOn: previousHandle); JobHandle applyConstraints = new ApplyAccumulatedConstraintsJob() { bodyVerts = softbodyData.bodyVerts, accumulatedDisplacements = softbodyData.accumulatedDisplacements }.Schedule(originalVerts.Length, 64, dependsOn: accumulateDistances); //Next, set the volume of the soft body JobHandle accumulateNormals = new AccumulateNormalsJob() { bodyVerts = softbodyData.bodyVerts, bodyTriangles = softbodyData.bodyTriangles, bodyNormals = softbodyData.bodyNormals }.Schedule(dependsOn: applyConstraints); JobHandle normalizeNormals = new NormalizeNormalsJob() { bodyNormals = softbodyData.bodyNormals }.Schedule(originalVerts.Length, 64, dependsOn: accumulateNormals); JobHandle calculateDilationDistance = new CalculateSurfaceAreaAndVolumeJob() { bodyVerts = softbodyData.bodyVerts, bodyTriangles = softbodyData.bodyTriangles, initialVolume = softbodyData.initialVolume, initialSurfaceArea = softbodyData.initialSurfaceArea, dilationDistance = softbodyData.dilationDistance }.Schedule(dependsOn: normalizeNormals); previousHandle = new ExtrudeNormalsJob() { bodyVerts = softbodyData.bodyVerts, bodyNormals = softbodyData.bodyNormals, dilationDistance = softbodyData.dilationDistance }.Schedule(originalVerts.Length, 64, dependsOn: calculateDilationDistance); } //Also sneak in a ground plane here: JobHandle groundPlaneHandle = new GroundCollideJob() { bodyVerts = softbodyData.bodyVerts, prevBodyVerts = softbodyData.prevBodyVerts, groundPlanePos = groundPlane.position, groundPlaneNormal = -groundPlane.forward }.Schedule(originalVerts.Length, 64, dependsOn: previousHandle); Profiler.EndSample(); groundPlaneHandle.Complete(); //Calculate the the position and rotation of the body for (int i = 0; i < softbodyData.bodyVerts.Length; i++) { softbodyData.kabschVerts[i] = new Vector4( softbodyData.bodyVerts[i].x, softbodyData.bodyVerts[i].y, softbodyData.bodyVerts[i].z, 1f); } ; softbodyData.kabschVerts.CopyTo(kabschVertsArray); Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVertsArray, transformFollowsRotation); transform.position = toWorldSpace.GetVector3(); transform.rotation = toWorldSpace.GetQuaternion(); //Move the points into local space for rendering JobHandle toLocalHandle = new ToLocalSpaceJob() { bodyVerts = softbodyData.bodyVerts, bodyNormals = softbodyData.bodyNormals, renderNormals = softbodyData.renderNormals, worldToLocal = transform.worldToLocalMatrix }.Schedule(originalVerts.Length, 64); toLocalHandle.Complete(); //Graphics softbodyData.bodyVerts.CopyTo(bodyVertsArray); softbodyData.renderNormals.CopyTo(renderNormalsArray); bodyMesh.vertices = bodyVertsArray; bodyMesh.normals = renderNormalsArray; bodyMesh.RecalculateBounds(); bodyMesh.UploadMeshData(false); }
IEnumerator CalibrateScreenLocation() { //Reset some stuff HyperMegaStuff.HyperMegaLines.drawer.dontClear = false; monitorPattern.SetActive(false); headsetPattern.SetActive(false); for (int i = 0; i < calibrationDevices.Length; i++) { if (calibrationDevices[i].isConnected) { calibrationDevices[i].resetMasks(); } calibrationDevices[i].undistortImage = false; // Stop undistorting the image because it's slow } whiteCircle.gameObject.SetActive(false); //1) Take a background subtraction shot of a black screen yield return(new WaitForSeconds(standardDelay)); float startTime = Time.unscaledTime; while (Time.unscaledTime < startTime + standardDelay * 10f) { //Take the max of the current image against the current subtraction image updateSubtractionBackgrounds(); yield return(null); } yield return(new WaitForSeconds(standardDelay)); //2) Unhide a white object monitorWhiteness.SetActive(true); yield return(new WaitForSeconds(standardDelay)); //3) Hit "Create Mask" // Create the binary masks from the subtracted images createBinaryMasks(monitorMaskThreshold); yield return(new WaitForSeconds(standardDelay)); monitorWhiteness.SetActive(false); //4) Start Moving the Circle Around whiteCircle.SetActive(true); yield return(new WaitForSeconds(standardDelay / 2f)); HyperMegaStuff.HyperMegaLines drawer = HyperMegaStuff.HyperMegaLines.drawer; drawer.dontClear = true; for (int i = 0; i < _realDots.Count; i++) { whiteCircle.transform.position = calibrationDotsParent.GetChild(i).position; int foundDots = 0; while (foundDots == 0) { yield return(new WaitForSeconds(standardDelay)); foundDots = 0; for (int j = 0; j < calibrationDevices.Length; j++) { Vector3 triangulatedDot = triangulate(j, drawer); if (triangulatedDot != Vector3.zero) { calibrationDevices[j].triangulatedDots[i] = triangulatedDot; foundDots++; } } } } whiteCircle.SetActive(false); drawer.dontClear = false; //Kabsch the Dots, move the screen! KabschSolver solver = new KabschSolver(); //Place the monitor based off of one of the webcams List <Vector3> _triangulatedDots = new List <Vector3>(), _monitorDots = new List <Vector3>(); for (int j = 0; j < _realDots.Count; j++) { if (calibrationDevices[0].triangulatedDots[j] != Vector3.zero) { _monitorDots.Add(calibrationDotsParent.GetChild(j).position); _triangulatedDots.Add(calibrationDevices[0].triangulatedDots[j]); } if (calibrationDevices[1].triangulatedDots[j] != Vector3.zero) { _monitorDots.Add(calibrationDotsParent.GetChild(j).position); _triangulatedDots.Add(calibrationDevices[1].triangulatedDots[j]); } } CalibrationMonitor.Transform(solver.SolveKabsch(_monitorDots, _triangulatedDots, 200)); for (int j = 0; j < _realDots.Count; j++) { _realDots[j] = calibrationDotsParent.GetChild(j).position; } monitorPattern.SetActive(true); headsetPattern.SetActive(true); }