public override void AccumulateCorrection(ref VRageMath.Vector3 correction, ref float weight)
{
Vector3D position = Parent.PositionAndOrientation.Translation;
BoundingBoxD box = new BoundingBoxD(position - Vector3D.One, position + Vector3D.One);
Vector3D currentMovement = Parent.ForwardVector * Parent.Speed;
if (Parent.Speed > 0.01f)
currentMovement.Normalize();
// Don't do any correction if we're not moving
if (currentMovement.LengthSquared() < 0.01)
{
return;
}
var entities = MyEntities.GetEntitiesInAABB(ref box);
foreach (var entity in entities)
{
var environmentItems = entity as MyEnvironmentItems;
if (environmentItems == null) continue;
environmentItems.GetItemsInRadius(ref position, 6.0f, m_trees);
foreach (var item in m_trees)
{
Vector3D dir = item - position;
var dist = dir.Normalize();
dir = Vector3D.Reject(currentMovement, dir);
dir.Y = 0.0f;
if (dir.Z * dir.Z + dir.X * dir.X < 0.1)
{
Vector3D dirLocal = Vector3D.TransformNormal(dir, Parent.PositionAndOrientationInverted);
dir = position - item;
dir = Vector3D.Cross(Vector3D.Up, dir);
if (dirLocal.X < 0)
dir = -dir;
}
dir.Normalize();
correction += (6f - dist) * Weight * dir;
if (!correction.IsValid())
{
System.Diagnostics.Debugger.Break();
}
}
m_trees.Clear();
}
weight += Weight;
entities.Clear();
}
public override void AccumulateCorrection(ref VRageMath.Vector3 correction, ref float weight)
{
// Don't do any correction if we're not moving
if (Parent.Speed < 0.01)
return;
Vector3D position = Parent.PositionAndOrientation.Translation;
// Find trees
Quaternion rotation = Quaternion.Identity;
HkShape sphereShape = new HkSphereShape(6);
MyPhysics.GetPenetrationsShape(sphereShape, ref position, ref rotation, m_trees, MyPhysics.CollisionLayers.NoVoxelCollisionLayer);
foreach (var tree in m_trees)
{
// Make sure the tree is actually a tree
if (tree.Body == null) continue;
MyPhysicsBody physicsBody = tree.Body.UserObject as MyPhysicsBody;
if (physicsBody == null) continue;
HkShape bodyShape = tree.Body.GetShape();
if (bodyShape.ShapeType != HkShapeType.StaticCompound)
continue;
// Get the static compound shape
HkStaticCompoundShape staticCompoundShape = (HkStaticCompoundShape)bodyShape;
int instanceId;
uint childKey;
staticCompoundShape.DecomposeShapeKey(tree.ShapeKey, out instanceId, out childKey);
// Get the local shape position, and add entity world position
Vector3D item = staticCompoundShape.GetInstanceTransform(instanceId).Translation;
item += physicsBody.GetWorldMatrix().Translation;
// Avoid tree
Vector3D dir = item - position;
var dist = dir.Normalize();
dir = Vector3D.Reject(Parent.ForwardVector, dir);
dir.Y = 0.0f;
if (dir.Z * dir.Z + dir.X * dir.X < 0.1)
{
Vector3D dirLocal = Vector3D.TransformNormal(dir, Parent.PositionAndOrientationInverted);
dir = position - item;
dir = Vector3D.Cross(Vector3D.Up, dir);
if (dirLocal.X < 0)
dir = -dir;
}
dir.Normalize();
correction += (6f - dist) * Weight * dir;
if (!correction.IsValid())
{
System.Diagnostics.Debugger.Break();
}
}
m_trees.Clear();
weight += Weight;
}