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; } }