@Test public void testInsideCollision() { Pose pos = Pose.zero(); IVector3 vel = Vector3.zero(); double center2Dribbler = 75; double obsRadius = getBotRadius() + getBallRadius(); double obsFront = center2Dribbler + getBallRadius(); BotID botID = BotID.createBotId(1, ETeamColor.YELLOW); BotCollisionObject colHandler = new BotCollisionObject(pos, vel, center2Dribbler, botID); Optional<ICollision> collision; collision = colHandler.getInsideCollision(Vector3.fromXY(0, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(10, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(obsFront, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(-10, 0)); assertThat(collision).isPresent(); assertThat(collision.get().getPos()).isEqualTo(Vector2.fromXY(-obsRadius, 0)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(-1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront - 1, 10)); assertThat(collision).isPresent(); assertThat(collision.get().getPos().x()).isCloseTo(obsFront, within(1e-5)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront - 1, -10)); assertThat(collision).isPresent(); assertThat(collision.get().getPos().x()).isCloseTo(obsFront, within(1e-5)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront - 1, 30)); assertThat(collision).isPresent(); assertThat(collision.get().getPos().x()).isCloseTo(obsFront, within(1e-5)); assertThat(collision.get().getNormal().normalizeNew()).isEqualTo(Vector2.fromXY(1, 0)); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront + 1, 0)); assertThat(collision).isNotPresent(); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront + 1, 10)); assertThat(collision).isNotPresent(); collision = colHandler.getInsideCollision(Vector3.fromXY(obsFront + 1, -10)); assertThat(collision).isNotPresent(); }
@Override public Optional<ICollision> getInsideCollision(final IVector3 prePos) { if (isInFront(prePos)) { double margin = lineCollision.isSticky() && lineCollision.getImpulse(prePos).getXYVector().isZeroVector() ? 10 : 0; if (frontTriangle.isPointInShape(prePos.getXYVector(), margin)) { IVector2 normal = Vector2.fromAngle(pose.getOrientation()); IVector2 colPos; colPos = frontLine.leadPointOf(prePos.getXYVector()); return Optional.of(new Collision(colPos, normal, lineCollision)); } return Optional.empty(); } return circleCollisionObject.getInsideCollision(prePos); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getInsideCollision(final IVector3 prePos) { if (isInFront(prePos)) { double margin = lineCollision.isSticky() && lineCollision.getImpulse(prePos).getXYVector().isZeroVector() ? 10 : 0; if (frontTriangle.isPointInShape(prePos.getXYVector(), margin)) { IVector2 normal = Vector2.fromAngle(pose.getOrientation()); IVector2 colPos; colPos = frontLine.leadPointOf(prePos.getXYVector()); return Optional.of(new Collision(colPos, normal, lineCollision)); } return Optional.empty(); } return circleCollisionObject.getInsideCollision(prePos); } }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getInsideCollision(final IVector3 prePos) { if (isInFront(prePos)) { double margin = lineCollision.isSticky() && lineCollision.getImpulse(prePos).getXYVector().isZeroVector() ? 10 : 0; if (frontTriangle.isPointInShape(prePos.getXYVector(), margin)) { IVector2 normal = Vector2.fromAngle(pose.getOrientation()); IVector2 colPos; colPos = frontLine.leadPointOf(prePos.getXYVector()); return Optional.of(new Collision(colPos, normal, lineCollision)); } return Optional.empty(); } return circleCollisionObject.getInsideCollision(prePos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getInsideCollision(final IVector3 prePos) { if (isInFront(prePos)) { double margin = lineCollision.isSticky() && lineCollision.getImpulse(prePos).getXYVector().isZeroVector() ? 10 : 0; if (frontTriangle.isPointInShape(prePos.getXYVector(), margin)) { IVector2 normal = Vector2.fromAngle(pose.getOrientation()); IVector2 colPos; colPos = frontLine.leadPointOf(prePos.getXYVector()); return Optional.of(new Collision(colPos, normal, lineCollision)); } return Optional.empty(); } return circleCollisionObject.getInsideCollision(prePos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); @Override IVector3 getVel(); @Override Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos); @Override Optional<ICollision> getInsideCollision(final IVector3 prePos); @Override IVector3 getImpulse(final IVector3 prePos); @Override BotID getBotID(); }
BotCollisionObject implements ICollisionObject { @Override public Optional<ICollision> getInsideCollision(final IVector3 prePos) { if (isInFront(prePos)) { double margin = lineCollision.isSticky() && lineCollision.getImpulse(prePos).getXYVector().isZeroVector() ? 10 : 0; if (frontTriangle.isPointInShape(prePos.getXYVector(), margin)) { IVector2 normal = Vector2.fromAngle(pose.getOrientation()); IVector2 colPos; colPos = frontLine.leadPointOf(prePos.getXYVector()); return Optional.of(new Collision(colPos, normal, lineCollision)); } return Optional.empty(); } return circleCollisionObject.getInsideCollision(prePos); } BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID);  BotCollisionObject(final Pose pose, final IVector3 vel, final double center2DribblerDist, final BotID botID,
			final boolean sticky,
			final double kickSpeed, final boolean chip); @Override IVector3 getVel(); @Override Optional<ICollision> getCollision(final IVector3 prePos, final IVector3 postPos); @Override Optional<ICollision> getInsideCollision(final IVector3 prePos); @Override IVector3 getImpulse(final IVector3 prePos); @Override BotID getBotID();  }