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;
}
public override void AccumulateCorrection(ref Vector3 correction, ref float weight)
{
if (base.Parent.Speed >= 0.01)
{
MatrixD positionAndOrientation = base.Parent.PositionAndOrientation;
Vector3D translation = positionAndOrientation.Translation;
Quaternion identity = Quaternion.Identity;
MyPhysics.GetPenetrationsShape((HkShape) new HkSphereShape(6f), ref translation, ref identity, this.m_trees, 9);
foreach (HkBodyCollision collision in this.m_trees)
{
if (collision.Body == null)
{
continue;
}
MyPhysicsBody userObject = collision.Body.UserObject as MyPhysicsBody;
if (userObject != null)
{
HkShape shape = collision.Body.GetShape();
if (shape.ShapeType == HkShapeType.StaticCompound)
{
int num;
uint num2;
HkStaticCompoundShape shape2 = (HkStaticCompoundShape)shape;
shape2.DecomposeShapeKey(collision.ShapeKey, out num, out num2);
Vector3D vectord2 = shape2.GetInstanceTransform(num).Translation + userObject.GetWorldMatrix().Translation;
Vector3D direction = vectord2 - translation;
double num3 = direction.Normalize();
direction = Vector3D.Reject(base.Parent.ForwardVector, direction);
direction.Y = 0.0;
if (((direction.Z * direction.Z) + (direction.X * direction.X)) < 0.1)
{
direction = translation - vectord2;
direction = Vector3D.Cross(Vector3D.Up, direction);
if (Vector3D.TransformNormal(direction, base.Parent.PositionAndOrientationInverted).X < 0.0)
{
direction = -direction;
}
}
direction.Normalize();
correction += ((6.0 - num3) * base.Weight) * direction;
if (!correction.IsValid())
{
Debugger.Break();
}
}
}
}
this.m_trees.Clear();
weight += base.Weight;
}
}