internal SolverFieldErrorScore ScorePose(ProxyForceFieldId id, Pose pose)
{
var state = new SolverFieldState();
var motion = new ProxyForceFieldMotion(pose, Pose.identity);
state.FrameReset(motion);
var oldPose = m_ProxyMotions[id];
m_ProxyMotions[id] = motion;
var def = m_ProxyDef[id];
def.isActive = true;
CalculateForces(ref state, def, id);
m_ProxyMotions[id] = oldPose;
state.errorScore.UpdateFinalScore();
return(state.errorScore);
}
bool CalculateForces(ref SolverFieldState state, ProxyForceField def, ProxyForceFieldId pi)
{
if ((!def.isActive) || (!def.allowedMotion.IsMoving()))
{
return(false);
}
//Debug.Log("Calculating forces...");
var proxyPose = state.worldMotion;
var didUpdate = false;
state.FrameReset(proxyPose);
foreach (var alignmentForce in def.alignments)
{
if ((!alignmentForce.isActive) || (!alignmentForce.targetProxyId.IsValid))
{
continue;
}
var hasTarget = (m_ProxyDef.ContainsKey(alignmentForce.targetProxyId));
if (hasTarget)
{
var targetDef = m_ProxyDef[alignmentForce.targetProxyId];
if ((!targetDef.isActive) || targetDef.isNotTracking)
{
hasTarget = false;
}
}
if (hasTarget)
{
var targetPose = m_ProxyMotions[alignmentForce.targetProxyId].location;
var appliedForce = alignmentForce.alignmentDefinition.AddAlignmentForce(ref state.forces, proxyPose.location, targetPose);
if (appliedForce)
{
didUpdate = true;
state.errorScore.scoreErrorForces += alignmentForce.alignmentDefinition.RatePoseMatch(proxyPose.location, targetPose);
state.errorScore.scoreErrorForcesCount++;
}
}
else
{
state.errorScore.scoreFatalErrorCount++; // fail, item is not trackable
}
}
var keys = m_ProxyDef.Keys;
foreach (var region in def.regions)
{
if ((!region.isActive) || (region.regionDefinition.regionType == ProxyRegionForceType.None))
{
continue;
}
// for each point in region:
var unitPnts = ProxyForceFieldPrimitive.UnitSamplesPointsForType(region.regionDefinition.shapePrimitive.primitiveType);
var dUnitPnt = 1.0f / ((float)unitPnts.Length);
var regionPose = region.proxyRelativePose.GetTransformedBy(proxyPose.location);
var regionPrim = new ProxyForceFieldPrimitivePosed(region.regionDefinition.shapePrimitive, regionPose);
var regionAttractorRequired = ((region.regionDefinition.regionType == ProxyRegionForceType.TowardsOccupiedEdge) ||
(region.regionDefinition.regionType == ProxyRegionForceType.TowardsOccupiedSpace));
var regionAttractorFound = false;
var responcesByUnitByResponce = EnsureAndClearCachedResponces(unitPnts.Length);
foreach (var otherProxy in m_ProxyDef)
{
if (otherProxy.Value.proxyId.Equals(def.proxyId))
{
continue; // same proxy, ignore self collision
}
if (!otherProxy.Value.isActive || (otherProxy.Value.regions == null))
{
continue;
}
var otherProxyPose = m_ProxyMotions[otherProxy.Key];
foreach (var otherRegion in otherProxy.Value.regions)
{
if (!otherRegion.isActive)
{
continue;
}
if ((region.requireOtherId != MarsTrackableId.InvalidId) &&
(region.requireOtherId != otherRegion.trackableId))
{
continue;
}
var responce = region.regionDefinition.GetResponceTypeToOtherRegion(otherRegion.regionDefinition);
if (responce == ProxyForceRegionResponce.Nothing)
{
// done
}
else
{
var otherRegionPose = otherRegion.proxyRelativePose.GetTransformedBy(otherProxyPose.location);
var otherRegionPrim = new ProxyForceFieldPrimitivePosed(otherRegion.regionDefinition.shapePrimitive, otherRegionPose);
var worthTest = false;
if (responce == ProxyForceRegionResponce.StayOutOf)
{
if (regionPrim.EstimateCollision(otherRegionPrim))
{
worthTest = true;
if (regionPrim.TryDecollisionSample(otherRegionPrim, out Vector3 decolMotion))
{
var moveVector = decolMotion * otherRegion.regionDefinition.fieldWeightScalar;
state.forces.AddDecollisionDirection(moveVector);
state.errorScore.scoreErrorRegion += decolMotion.magnitude;
state.errorScore.scoreErrorRegionCount++;
didUpdate = true;
worthTest = false; // already done
}
}
}
else if ((responce == ProxyForceRegionResponce.AlignWithEdge) || (responce == ProxyForceRegionResponce.AlignWithVolume))
{
if (regionPrim.EstimateAttraction(otherRegionPrim))
{
worthTest = true;
}
}
if (worthTest)
{
var pointIndex = -1;
foreach (var unitPnt in unitPnts)
{
pointIndex++;
var refUnitPoint = regionPrim.Primitive.UnitFromReferenceVector(unitPnt);
var worldPoint = regionPrim.WorldFromUnitPoint(refUnitPoint * 0.99f);
var regionSample = regionPrim.SampleDistanceFieldWorld(worldPoint, ProxyForceFieldPrimitive.ShapeSample.Fill);
var otherSample = otherRegionPrim.SampleDistanceFieldWorld(worldPoint, (responce == ProxyForceRegionResponce.AlignWithEdge) ? ProxyForceFieldPrimitive.ShapeSample.Edge : ProxyForceFieldPrimitive.ShapeSample.Fill);
otherSample = otherSample.AdjustedForResponce(responce, worldPoint, regionPose);
otherSample.RegionToSurface = regionSample.ToSurface;
otherSample.SampleWeight = otherRegion.regionDefinition.fieldWeightScalar;
if (otherSample.HasSample)
{
if (regionAttractorRequired)
{
regionAttractorFound = true;
}
//Debug.Log("Responce=" + responce);
//Debug.DrawLine(worldPoint, worldPoint + (otherSample.ToSurface * 1.0f), Color.white);
var s = responcesByUnitByResponce[pointIndex][(int)responce];
var t = s.CombinedWith(otherSample);
responcesByUnitByResponce[pointIndex][(int)responce] = t;
}
// end of a point
}
}
// end of region to region test
}
} // end of other region
} // end of other proxy
// Now apply values from all points in the region:
if (regionAttractorRequired && !regionAttractorFound)
{
state.errorScore.scoreFatalErrorCount++; // was missing an attractor!
}
for (int pointIndex = 0; pointIndex < unitPnts.Length; pointIndex++)
{
for (var ri = 0; ri < (int)ProxyForceRegionResponce.COUNT_TYPES; ri++)
{
var s = responcesByUnitByResponce[pointIndex][ri];
if (s.HasSample)
{
var responce = (ProxyForceRegionResponce)ri;
var responceWeight = ProxyForceRegionDefintion.GetResponceWeight(responce);
var cw = s.SampleWeight * dUnitPnt * responceWeight;
var qb = s.ToSurface;//NOTE: we are not taking the SDF distance which would be: s.ToSurface - s.RegionToSurface;
var qw = qb * cw;
if (ShowForcesAsLines)
{
Debug.DrawLine(s.WorldPoint, s.WorldPoint + s.ToSurface, Color.grey);
Debug.DrawLine(s.WorldPoint, s.WorldPoint + qw, Color.white);
}
state.forces.AddForceAtWorldPoint(s.WorldPoint, qw);
didUpdate = true;
state.errorScore.scoreErrorRegion += s.Distance;
state.errorScore.scoreErrorRegionCount++;
}
}
}
}
state.didUpdate = didUpdate;
return(didUpdate);
}