public void Execute(
Entity entity, int jobIndex,
[WriteOnly] ref WallHitResultData result,
[ReadOnly] ref GroundHitSphereData sphere,
[WriteOnly] ref Translation pos,
[WriteOnly] ref Rotation rot
)
{
//var rtf = new RigidTransform( rot.Value, pos.Value );
var hitInput = new PointDistanceInput
{
Position = pos.Value,//math.transform( rtf, sphere.Center ),
MaxDistance = sphere.Distance,
Filter = sphere.Filter,
};
//var isHit = this.CollisionWorld.CalculateDistance( hitInput, ref a );// 自身のコライダを除外できればシンプルになるんだが…
var collector = new ClosestHitExcludeSelfCollector <DistanceHit>(sphere.Distance, entity, this.MainEntities);
//var collector = new ClosestHitCollector<DistanceHit>( sphere.Distance );
var isHit = this.CollisionWorld.CalculateDistance(hitInput, ref collector);
if (collector.NumHits == 0)
{
return;
}
result.IsHit = true;
var n = collector.ClosestHit.SurfaceNormal;
var p = collector.ClosestHit.Position;
pos.Value = p + n * sphere.Distance;
var right = math.mul(rot.Value, new float3(1.0f, 0.0f, 0.0f));
var forward = math.cross(n, right);
var safe_forward = math.select(math.forward(rot.Value), forward, math.dot(right, n) > 0.001f);
rot.Value = quaternion.LookRotation(safe_forward, n);
}
bool raycastHitToWall_
(
ref PhysicsVelocity v, ref float3 pos, ref quaternion rot, float dt,
float3 origin, float3 gndray, float bodySize, float3 fwddir,
Entity ent, CollisionFilter filter,
ComponentDataFromEntity <Bone.MainEntityLinkData> mainEntities
)
{
var h = raycast(ref this.CollisionWorld, origin, gndray, ent, filter, mainEntities);
//var (isHit, hit) = raycast( ref this.CollisionWorld, origin, gndray, ent, filter );
if (h.isHit)
{
var newposrot = caluclateWallPosture
//var (newpos, newrot) = caluclateWallPosture
(origin, h.hit.Position, h.hit.SurfaceNormal, fwddir, bodySize);
var rdt = math.rcp(dt);
v.Linear = (newposrot.pos - pos) * rdt;
//pos = newposrot.pos;
//var invprev = math.inverse( newposrot.rot );
//var drot = math.mul( invprev, rot );
//var angle = math.acos( drot.value.w ) * 2.0f;
//var sin = math.sin( angle );
//var axis = drot.value.As_float3() * math.rcp( sin );
//var invprev = math.inverse( newposrot.rot );
//var drot = math.mul( invprev, rot );
//var axis = drot.value.As_float3();
//var angle = math.lengthsq( drot );
//v.Angular = axis * ( angle * rdt );
v.Angular = float3.zero;
rot = newposrot.rot;
}
return(h.isHit);
HitFlagAndResult raycast
//( bool isHit, RaycastHit hit) raycast
(
ref PhysicsWorld cw, float3 origin_, float3 ray_, Entity ent_, CollisionFilter filter_,
ComponentDataFromEntity <Bone.MainEntityLinkData> mainEntities_
)
{
var hitInput = new RaycastInput
{
Start = origin_,
End = origin_ + ray_,
Filter = filter_,
};
var collector = new ClosestHitExcludeSelfCollector <RaycastHit>(1.0f, ent, mainEntities_);
//var collector = new ClosestHitCollector<RaycastHit>( 1.0f );
/*var isHit = */
cw.CastRay(hitInput, ref collector);
return(new HitFlagAndResult {
isHit = collector.NumHits > 0, hit = collector.ClosestHit
});
//return (collector.NumHits > 0, collector.ClosestHit);
}
PosAndRot caluclateWallPosture
//( float3 newpos, quaternion newrot) caluclateWallPosture
(float3 o, float3 p, float3 n, float3 up, float r)
{
var f = p - o;
var w = f - math.dot(f, n) * n;
var upsign = math.sign(math.dot(up, w));
var newfwd = math.select(up, math.normalize(w * upsign), math.lengthsq(w) > 0.001f);
//var newfwd = math.lengthsq(w) > 0.001f ? math.normalize( w * math.sign( math.dot( up, w ) ) ) : up;
var newpos = p + n * r;
var newrot = quaternion.LookRotation(newfwd, n);
return(new PosAndRot {
pos = newpos, rot = newrot
});
//return (newpos, newrot);
}
}
