diff --git a/.gitignore b/.gitignore index e2d5c7b..8f67fa6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -/.gradle + /.idea /build *.iml @@ -6,5 +6,4 @@ *.log /local.properties /deps -gradle .run diff --git a/.gradle/8.0/checksums/checksums.lock b/.gradle/8.0/checksums/checksums.lock new file mode 100644 index 0000000..1e4165a Binary files /dev/null and b/.gradle/8.0/checksums/checksums.lock differ diff --git a/.gradle/8.0/checksums/md5-checksums.bin b/.gradle/8.0/checksums/md5-checksums.bin new file mode 100644 index 0000000..5a47538 Binary files /dev/null and b/.gradle/8.0/checksums/md5-checksums.bin differ diff --git a/.gradle/8.0/checksums/sha1-checksums.bin b/.gradle/8.0/checksums/sha1-checksums.bin new file mode 100644 index 0000000..b403f42 Binary files /dev/null and b/.gradle/8.0/checksums/sha1-checksums.bin differ diff --git a/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$BundleAccessors.class b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$BundleAccessors.class new file mode 100644 index 0000000..06d3f2b Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$BundleAccessors.class differ diff --git a/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$PluginAccessors.class b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$PluginAccessors.class new file mode 100644 index 0000000..2214ec1 Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$PluginAccessors.class differ diff --git a/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$VersionAccessors.class b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$VersionAccessors.class new file mode 100644 index 0000000..9aa208a Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs$VersionAccessors.class differ diff --git a/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs.class b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs.class new file mode 100644 index 0000000..7b7aa2c Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/classes/org/gradle/accessors/dm/LibrariesForLibs.class differ diff --git a/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/sources/org/gradle/accessors/dm/LibrariesForLibs.java b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/sources/org/gradle/accessors/dm/LibrariesForLibs.java new file mode 100644 index 0000000..786c1d4 --- /dev/null +++ b/.gradle/8.0/dependencies-accessors/acfe0728e9071b59fa425e3ceae079fe41466fa5/sources/org/gradle/accessors/dm/LibrariesForLibs.java @@ -0,0 +1,73 @@ +package org.gradle.accessors.dm; + +import org.gradle.api.NonNullApi; +import org.gradle.api.artifacts.MinimalExternalModuleDependency; +import org.gradle.plugin.use.PluginDependency; +import org.gradle.api.artifacts.ExternalModuleDependencyBundle; +import org.gradle.api.artifacts.MutableVersionConstraint; +import org.gradle.api.provider.Provider; +import org.gradle.api.model.ObjectFactory; +import org.gradle.api.provider.ProviderFactory; +import org.gradle.api.internal.catalog.AbstractExternalDependencyFactory; +import org.gradle.api.internal.catalog.DefaultVersionCatalog; +import java.util.Map; +import org.gradle.api.internal.attributes.ImmutableAttributesFactory; +import org.gradle.api.internal.artifacts.dsl.CapabilityNotationParser; +import javax.inject.Inject; + +/** + * A catalog of dependencies accessible via the `libs` extension. +*/ +@NonNullApi +public class LibrariesForLibs extends AbstractExternalDependencyFactory { + + private final AbstractExternalDependencyFactory owner = this; + private final VersionAccessors vaccForVersionAccessors = new VersionAccessors(providers, config); + private final BundleAccessors baccForBundleAccessors = new BundleAccessors(objects, providers, config, attributesFactory, capabilityNotationParser); + private final PluginAccessors paccForPluginAccessors = new PluginAccessors(providers, config); + + @Inject + public LibrariesForLibs(DefaultVersionCatalog config, ProviderFactory providers, ObjectFactory objects, ImmutableAttributesFactory attributesFactory, CapabilityNotationParser capabilityNotationParser) { + super(config, providers, objects, attributesFactory, capabilityNotationParser); + } + + /** + * Returns the group of versions at versions + */ + public VersionAccessors getVersions() { return vaccForVersionAccessors; } + + /** + * Returns the group of bundles at bundles + */ + public BundleAccessors getBundles() { return baccForBundleAccessors; } + + /** + * Returns the group of plugins at plugins + */ + public PluginAccessors getPlugins() { return paccForPluginAccessors; } + + public static class VersionAccessors extends VersionFactory { + + public VersionAccessors(ProviderFactory providers, DefaultVersionCatalog config) { super(providers, config); } + + } + + public static class BundleAccessors extends BundleFactory { + + public BundleAccessors(ObjectFactory objects, ProviderFactory providers, DefaultVersionCatalog config, ImmutableAttributesFactory attributesFactory, CapabilityNotationParser capabilityNotationParser) { super(objects, providers, config, attributesFactory, capabilityNotationParser); } + + } + + public static class PluginAccessors extends PluginFactory { + + public PluginAccessors(ProviderFactory providers, DefaultVersionCatalog config) { super(providers, config); } + + /** + * Creates a plugin provider for korge to the plugin id 'com.soywiz.korge' + * This plugin was declared in catalog libs.versions.toml + */ + public Provider getKorge() { return createPlugin("korge"); } + + } + +} diff --git a/.gradle/8.0/dependencies-accessors/dependencies-accessors.lock b/.gradle/8.0/dependencies-accessors/dependencies-accessors.lock new file mode 100644 index 0000000..21cb7c1 Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/dependencies-accessors.lock differ diff --git a/.gradle/8.0/dependencies-accessors/executionHistory.bin b/.gradle/8.0/dependencies-accessors/executionHistory.bin new file mode 100644 index 0000000..57e3b76 Binary files /dev/null and b/.gradle/8.0/dependencies-accessors/executionHistory.bin differ diff --git a/.gradle/8.0/dependencies-accessors/gc.properties b/.gradle/8.0/dependencies-accessors/gc.properties new file mode 100644 index 0000000..e69de29 diff --git a/.gradle/8.0/executionHistory/executionHistory.bin b/.gradle/8.0/executionHistory/executionHistory.bin new file mode 100644 index 0000000..4923185 Binary files /dev/null and b/.gradle/8.0/executionHistory/executionHistory.bin differ diff --git a/.gradle/8.0/executionHistory/executionHistory.lock b/.gradle/8.0/executionHistory/executionHistory.lock new file mode 100644 index 0000000..e8e397d Binary files /dev/null and b/.gradle/8.0/executionHistory/executionHistory.lock differ diff --git a/.gradle/8.0/fileChanges/last-build.bin b/.gradle/8.0/fileChanges/last-build.bin new file mode 100644 index 0000000..f76dd23 Binary files /dev/null and b/.gradle/8.0/fileChanges/last-build.bin differ diff --git a/.gradle/8.0/fileHashes/fileHashes.bin b/.gradle/8.0/fileHashes/fileHashes.bin new file mode 100644 index 0000000..08d363e Binary files /dev/null and b/.gradle/8.0/fileHashes/fileHashes.bin differ diff --git a/.gradle/8.0/fileHashes/fileHashes.lock b/.gradle/8.0/fileHashes/fileHashes.lock new file mode 100644 index 0000000..1a5d44b Binary files /dev/null and b/.gradle/8.0/fileHashes/fileHashes.lock differ diff --git a/.gradle/8.0/fileHashes/resourceHashesCache.bin b/.gradle/8.0/fileHashes/resourceHashesCache.bin new file mode 100644 index 0000000..14b3d77 Binary files /dev/null and b/.gradle/8.0/fileHashes/resourceHashesCache.bin differ diff --git a/.gradle/8.0/gc.properties b/.gradle/8.0/gc.properties new file mode 100644 index 0000000..e69de29 diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock new file mode 100644 index 0000000..f892418 Binary files /dev/null and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/.gradle/buildOutputCleanup/cache.properties b/.gradle/buildOutputCleanup/cache.properties new file mode 100644 index 0000000..b444936 --- /dev/null +++ b/.gradle/buildOutputCleanup/cache.properties @@ -0,0 +1,2 @@ +#Sat Mar 08 12:59:24 CET 2025 +gradle.version=8.0 diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin new file mode 100644 index 0000000..9cd922f Binary files /dev/null and b/.gradle/buildOutputCleanup/outputFiles.bin differ diff --git a/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/buildfingerprint.bin b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/buildfingerprint.bin new file mode 100644 index 0000000..19e3033 Binary files /dev/null and b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/buildfingerprint.bin differ diff --git a/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/entry.bin b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/entry.bin new file mode 100644 index 0000000..24b3ce7 Binary files /dev/null and b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/entry.bin differ diff --git a/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/projectfingerprint.bin b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/projectfingerprint.bin new file mode 100644 index 0000000..79fd0db --- /dev/null +++ b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/projectfingerprint.bin @@ -0,0 +1 @@ +ÿÿÿÿ \ No newline at end of file diff --git a/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/work.bin b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/work.bin new file mode 100644 index 0000000..9536b2b Binary files /dev/null and b/.gradle/configuration-cache/6n82baauj8thwwmu9hpr5gv2d/work.bin differ diff --git a/.gradle/configuration-cache/configuration-cache.lock b/.gradle/configuration-cache/configuration-cache.lock new file mode 100644 index 0000000..0c3a2b4 Binary files /dev/null and b/.gradle/configuration-cache/configuration-cache.lock differ diff --git a/.gradle/configuration-cache/gc.properties b/.gradle/configuration-cache/gc.properties new file mode 100644 index 0000000..e69de29 diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe new file mode 100644 index 0000000..ab5ac5b Binary files /dev/null and b/.gradle/file-system.probe differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-commonMain-Fr5MHg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-commonMain-Fr5MHg.klib new file mode 100644 index 0000000..d739964 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-commonMain-Fr5MHg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-concurrentMain-Fr5MHg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-concurrentMain-Fr5MHg.klib new file mode 100644 index 0000000..530fed3 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-concurrentMain-Fr5MHg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-nativeMain-Fr5MHg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-nativeMain-Fr5MHg.klib new file mode 100644 index 0000000..42c633d Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kds-kds-4.0.1-nativeMain-Fr5MHg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-commonMain-5A-0FA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-commonMain-5A-0FA.klib new file mode 100644 index 0000000..8cb5be0 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-commonMain-5A-0FA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-darwinMain-4fp_IA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-darwinMain-4fp_IA.klib new file mode 100644 index 0000000..27a993a Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-darwinMain-4fp_IA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-linuxMain-5A-0FA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-linuxMain-5A-0FA.klib new file mode 100644 index 0000000..42475d2 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-linuxMain-5A-0FA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-nativeMain-5A-0FA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-nativeMain-5A-0FA.klib new file mode 100644 index 0000000..d35d57f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klock-klock-4.0.1-nativeMain-5A-0FA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-commonMain-F_tLlg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-commonMain-F_tLlg.klib new file mode 100644 index 0000000..b245c07 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-commonMain-F_tLlg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-darwinMain-Hgd2_A.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-darwinMain-Hgd2_A.klib new file mode 100644 index 0000000..f6a06fc Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-darwinMain-Hgd2_A.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-linuxMain-F_tLlg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-linuxMain-F_tLlg.klib new file mode 100644 index 0000000..37aeb81 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-linuxMain-F_tLlg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-nativeMain-F_tLlg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-nativeMain-F_tLlg.klib new file mode 100644 index 0000000..a8d9dcf Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.klogger-klogger-4.0.1-nativeMain-F_tLlg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-commonMain-gp9Nkw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-commonMain-gp9Nkw.klib new file mode 100644 index 0000000..db39ae7 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-commonMain-gp9Nkw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-nativeMain-gp9Nkw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-nativeMain-gp9Nkw.klib new file mode 100644 index 0000000..738e15a Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-nativeMain-gp9Nkw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-posixMain-gp9Nkw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-posixMain-gp9Nkw.klib new file mode 100644 index 0000000..ea40c0c Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.kmem-kmem-4.0.1-posixMain-gp9Nkw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-commonMain-6QdVZg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-commonMain-6QdVZg.klib new file mode 100644 index 0000000..5f1924b Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-commonMain-6QdVZg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-kbPpgQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-kbPpgQ.klib new file mode 100644 index 0000000..b84eb0f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-kbPpgQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-yhgVTw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-yhgVTw.klib new file mode 100644 index 0000000..b84eb0f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMacosMain-yhgVTw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMain-yhgVTw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMain-yhgVTw.klib new file mode 100644 index 0000000..5bc11ad Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-iosMain-yhgVTw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-linuxMain-6QdVZg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-linuxMain-6QdVZg.klib new file mode 100644 index 0000000..a1915c3 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-linuxMain-6QdVZg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-macosMain-kbPpgQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-macosMain-kbPpgQ.klib new file mode 100644 index 0000000..6c5bfeb Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-macosMain-kbPpgQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-nativeMain-6QdVZg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-nativeMain-6QdVZg.klib new file mode 100644 index 0000000..a7e9979 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korau-korau-4.0.1-nativeMain-6QdVZg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korcoutines-korcoutines-4.0.1-commonMain-psXoyg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korcoutines-korcoutines-4.0.1-commonMain-psXoyg.klib new file mode 100644 index 0000000..b55b7b9 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korcoutines-korcoutines-4.0.1-commonMain-psXoyg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-commonMain-sY4YGg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-commonMain-sY4YGg.klib new file mode 100644 index 0000000..96e06c6 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-commonMain-sY4YGg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-darwinMain-32wIIw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-darwinMain-32wIIw.klib new file mode 100644 index 0000000..3945f55 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-darwinMain-32wIIw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-linuxMain-sY4YGg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-linuxMain-sY4YGg.klib new file mode 100644 index 0000000..df59258 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-linuxMain-sY4YGg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-nativeMain-sY4YGg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-nativeMain-sY4YGg.klib new file mode 100644 index 0000000..f00473f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korge2-korge-4.0.1-nativeMain-sY4YGg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-commonMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-commonMain-wHCTGw.klib new file mode 100644 index 0000000..eb38d5b Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-commonMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-concurrentMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-concurrentMain-wHCTGw.klib new file mode 100644 index 0000000..f84ed54 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-concurrentMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-DCaThQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-DCaThQ.klib new file mode 100644 index 0000000..9fa830c Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-DCaThQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-PKE7aw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-PKE7aw.klib new file mode 100644 index 0000000..4b50d85 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMain-PKE7aw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMobileMain-PKE7aw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMobileMain-PKE7aw.klib new file mode 100644 index 0000000..4b50d85 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-darwinMobileMain-PKE7aw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-DCaThQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-DCaThQ.klib new file mode 100644 index 0000000..1534da3 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-DCaThQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-PKE7aw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-PKE7aw.klib new file mode 100644 index 0000000..0ed2e68 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMacosMain-PKE7aw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMain-PKE7aw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMain-PKE7aw.klib new file mode 100644 index 0000000..3dfde93 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosMain-PKE7aw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosTvosMain-PKE7aw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosTvosMain-PKE7aw.klib new file mode 100644 index 0000000..4b50d85 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-iosTvosMain-PKE7aw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-jvmAndroidMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-jvmAndroidMain-wHCTGw.klib new file mode 100644 index 0000000..f84ed54 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-jvmAndroidMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-linuxMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-linuxMain-wHCTGw.klib new file mode 100644 index 0000000..5a8b164 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-linuxMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-macosMain-DCaThQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-macosMain-DCaThQ.klib new file mode 100644 index 0000000..c20f77e Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-macosMain-DCaThQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-nativeMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-nativeMain-wHCTGw.klib new file mode 100644 index 0000000..fc8b4e1 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-nativeMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-posixMain-wHCTGw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-posixMain-wHCTGw.klib new file mode 100644 index 0000000..dfcf54d Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korgw-korgw-4.0.1-posixMain-wHCTGw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-commonMain-ymuioA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-commonMain-ymuioA.klib new file mode 100644 index 0000000..bba1503 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-commonMain-ymuioA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-HT1yfg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-HT1yfg.klib new file mode 100644 index 0000000..ebb7a56 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-HT1yfg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-tCnBSw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-tCnBSw.klib new file mode 100644 index 0000000..ebb7a56 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMain-tCnBSw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMobileMain-HT1yfg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMobileMain-HT1yfg.klib new file mode 100644 index 0000000..017c20b Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-darwinMobileMain-HT1yfg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-linuxMain-ymuioA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-linuxMain-ymuioA.klib new file mode 100644 index 0000000..ab225e5 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-linuxMain-ymuioA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-macosMain-tCnBSw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-macosMain-tCnBSw.klib new file mode 100644 index 0000000..baacd33 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-macosMain-tCnBSw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-nativeMain-ymuioA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-nativeMain-ymuioA.klib new file mode 100644 index 0000000..83c0a67 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korim-korim-4.0.1-nativeMain-ymuioA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korinject-korinject-4.0.1-commonMain-XbJ02w.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korinject-korinject-4.0.1-commonMain-XbJ02w.klib new file mode 100644 index 0000000..7e1501b Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korinject-korinject-4.0.1-commonMain-XbJ02w.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-commonMain-Ychfxw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-commonMain-Ychfxw.klib new file mode 100644 index 0000000..d36ae3d Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-commonMain-Ychfxw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-concurrentMain-Ychfxw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-concurrentMain-Ychfxw.klib new file mode 100644 index 0000000..c0d7990 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-concurrentMain-Ychfxw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-dUXqDg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-dUXqDg.klib new file mode 100644 index 0000000..66c3f31 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-dUXqDg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-vkoItQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-vkoItQ.klib new file mode 100644 index 0000000..66c3f31 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-darwinMain-vkoItQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-iosMain-dUXqDg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-iosMain-dUXqDg.klib new file mode 100644 index 0000000..b98c50b Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-iosMain-dUXqDg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-linuxMain-Ychfxw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-linuxMain-Ychfxw.klib new file mode 100644 index 0000000..b3bf453 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-linuxMain-Ychfxw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-macosMain-vkoItQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-macosMain-vkoItQ.klib new file mode 100644 index 0000000..3d17e65 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-macosMain-vkoItQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-nativeMain-Ychfxw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-nativeMain-Ychfxw.klib new file mode 100644 index 0000000..35a1fef Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-nativeMain-Ychfxw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-posixMain-Ychfxw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-posixMain-Ychfxw.klib new file mode 100644 index 0000000..3de7185 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korio-korio-4.0.1-posixMain-Ychfxw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korma-korma-4.0.1-commonMain-D80Xjg.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korma-korma-4.0.1-commonMain-D80Xjg.klib new file mode 100644 index 0000000..a67179a Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korma-korma-4.0.1-commonMain-D80Xjg.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-commonMain-Wn1T6Q.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-commonMain-Wn1T6Q.klib new file mode 100644 index 0000000..cfaef3c Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-commonMain-Wn1T6Q.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-nativeMain-Wn1T6Q.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-nativeMain-Wn1T6Q.klib new file mode 100644 index 0000000..8f1a8d4 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.korte-korte-4.0.1-nativeMain-Wn1T6Q.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-commonMain-G7Vtdw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-commonMain-G7Vtdw.klib new file mode 100644 index 0000000..37b3431 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-commonMain-G7Vtdw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-darwinMain-uv4F1g.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-darwinMain-uv4F1g.klib new file mode 100644 index 0000000..74ba359 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-darwinMain-uv4F1g.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-linuxMain-G7Vtdw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-linuxMain-G7Vtdw.klib new file mode 100644 index 0000000..267ee55 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/com.soywiz.korlibs.krypto-krypto-4.0.1-linuxMain-G7Vtdw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-commonMain-nfvfsw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-commonMain-nfvfsw.klib new file mode 100644 index 0000000..ae8aaa3 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-commonMain-nfvfsw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-nativeMain-nfvfsw.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-nativeMain-nfvfsw.klib new file mode 100644 index 0000000..e948e5f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-atomicfu-0.20.2-nativeMain-nfvfsw.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-commonMain-UZihAA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-commonMain-UZihAA.klib new file mode 100644 index 0000000..576b99a Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-commonMain-UZihAA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-concurrentMain-UZihAA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-concurrentMain-UZihAA.klib new file mode 100644 index 0000000..22bb4bd Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-concurrentMain-UZihAA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeDarwinMain-NVh8pQ.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeDarwinMain-NVh8pQ.klib new file mode 100644 index 0000000..e9aea79 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeDarwinMain-NVh8pQ.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeMain-UZihAA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeMain-UZihAA.klib new file mode 100644 index 0000000..9efffb0 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeMain-UZihAA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeOtherMain-UZihAA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeOtherMain-UZihAA.klib new file mode 100644 index 0000000..baa151f Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-coroutines-core-1.7.1-nativeOtherMain-UZihAA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-core-1.5.1-commonMain-JVxaSA.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-core-1.5.1-commonMain-JVxaSA.klib new file mode 100644 index 0000000..e37abc1 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-core-1.5.1-commonMain-JVxaSA.klib differ diff --git a/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-json-1.5.1-commonMain-xO3r_w.klib b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-json-1.5.1-commonMain-xO3r_w.klib new file mode 100644 index 0000000..04cd629 Binary files /dev/null and b/.gradle/kotlin/kotlinTransformedMetadataLibraries/org.jetbrains.kotlinx-kotlinx-serialization-json-1.5.1-commonMain-xO3r_w.klib differ diff --git a/.gradle/vcs-1/gc.properties b/.gradle/vcs-1/gc.properties new file mode 100644 index 0000000..e69de29 diff --git a/gradle/libs.versions.toml b/gradle/libs.versions.toml new file mode 100644 index 0000000..5ba6c90 --- /dev/null +++ b/gradle/libs.versions.toml @@ -0,0 +1,3 @@ +[plugins] +korge = { id = "com.soywiz.korge", version = "4.0.1" } +#korge = { id = "com.soywiz.korge", version = "999.0.0.999" } diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..ccebba7 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..42defcc --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,6 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.0-bin.zip +networkTimeout=10000 +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/modules/korge-box2d/.gitarchive b/modules/korge-box2d/.gitarchive new file mode 100644 index 0000000..f4caccc --- /dev/null +++ b/modules/korge-box2d/.gitarchive @@ -0,0 +1 @@ +v0.1.2 \ No newline at end of file diff --git a/modules/korge-box2d/.gitignore b/modules/korge-box2d/.gitignore new file mode 100644 index 0000000..2bbdeb3 --- /dev/null +++ b/modules/korge-box2d/.gitignore @@ -0,0 +1,4 @@ +/.idea +/.gradle +/build +/build.gradle diff --git a/modules/korge-box2d/LICENSE b/modules/korge-box2d/LICENSE new file mode 100644 index 0000000..d076f66 --- /dev/null +++ b/modules/korge-box2d/LICENSE @@ -0,0 +1,27 @@ +------------------------------------------------------------------------------- +-- jbox2d license ------------------------------------------------------------- +------------------------------------------------------------------------------- + +Copyright (c) 2014, Daniel Murphy +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/modules/korge-box2d/README.md b/modules/korge-box2d/README.md new file mode 100644 index 0000000..6aa3d6d --- /dev/null +++ b/modules/korge-box2d/README.md @@ -0,0 +1 @@ +## Port of jbox2d diff --git a/modules/korge-box2d/kproject.yml b/modules/korge-box2d/kproject.yml new file mode 100644 index 0000000..c530e57 --- /dev/null +++ b/modules/korge-box2d/kproject.yml @@ -0,0 +1,2 @@ +dependencies: +- maven::common::com.soywiz.korlibs.korge2:korge diff --git a/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/KtreeSerializer.kt b/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/KtreeSerializer.kt new file mode 100644 index 0000000..fb6c5c5 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/KtreeSerializer.kt @@ -0,0 +1,129 @@ +package korlibs.korge.box2d + +/* +import korlibs.korge.view.ktree.* + +@ThreadLocal +var KTreeSerializer.box2dWorld by Extra.PropertyThis { null } + +object PhysicsKTreeSerializerExtension : KTreeSerializerExtension("physics") { + override fun complete(serializer: KTreeSerializer, view: View) { + //serializer.box2dWorld?.world?.forEachBody { println("it.linearVelocityY: ${it.linearVelocityY}") } + serializer.box2dWorld?.update(0.0.milliseconds) + serializer.box2dWorld?.world?.forEachBody { + if (!it.didReset) { + it.didReset = true + it.type = it.bodyDef.type + it.linearVelocityX = it.bodyDef.linearVelocity.x + it.linearVelocityY = it.bodyDef.linearVelocity.y + it.gravityScale = it.bodyDef.gravityScale + it.angularVelocity = it.bodyDef.angularVelocity + it.isSleepingAllowed = it.bodyDef.allowSleep + it.isAwake = it.bodyDef.awake + it.isFixedRotation = it.bodyDef.fixedRotation + it.isBullet = it.bodyDef.bullet + } + //println("it.linearVelocityY: ${it.linearVelocityY}") + } + } + + override fun setProps(serializer: KTreeSerializer, view: View, xml: Xml) { + //println("PhysicsKTreeSerializerExtension.setProps") + val body = view.registerBodyWithFixture( + world = serializer.box2dWorld?.world, + type = xml.strNull("type")?.let { BodyType[it] } ?: BodyType.STATIC, + linearVelocityX = xml.float("linearVelocityX", 0f), + linearVelocityY = xml.float("linearVelocityY", 0f), + gravityScale = xml.float("gravityScale", 1f), + angularVelocity = xml.float("angularVelocity", 0f), + allowSleep = xml.boolean("isSleepingAllowed", true), + awake = xml.boolean("isAwake", true), + fixedRotation = xml.boolean("isFixedRotation", false), + bullet = xml.boolean("isBullet", false), + friction = xml.float("friction", 0f), + density = xml.float("density", 1f), + restitution = xml.float("restitution", 0f), + isSensor = xml.boolean("isSensor", false), + active = xml.boolean("isActive", true) + ).body + body?.didReset = false + } + + override fun getProps(serializer: KTreeSerializer, view: View): Map? { + val body = view.body ?: return null + val fixture = body.m_fixtureList + //println("PhysicsKTreeSerializerExtension.getProps") + return LinkedHashMap().apply { + if (body.type != BodyType.STATIC) this["type"] = body.type + if (body.linearVelocityX != 0f) this["linearVelocityX"] = body.linearVelocityX + if (body.linearVelocityY != 0f) this["linearVelocityY"] = body.linearVelocityY + if (body.gravityScale != 1f) this["gravityScale"] = body.gravityScale + if (body.angularVelocity != 0f) this["angularVelocity"] = body.angularVelocity + if (!body.isSleepingAllowed) this["isSleepingAllowed"] = body.isSleepingAllowed + if (!body.isAwake) this["isAwake"] = body.isAwake + if (body.isFixedRotation) this["isFixedRotation"] = body.isFixedRotation + if (body.isBullet) this["isBullet"] = body.isBullet + if (!body.isActive) this["isActive"] = body.isActive + if (fixture != null) { + if (fixture.isSensor) this["isSensor"] = fixture.isSensor + if (fixture.friction != 0f) this["friction"] = fixture.friction + if (fixture.density != 1f) this["density"] = fixture.density + if (fixture.restitution != 0f) this["restitution"] = fixture.restitution + } + } + } +} +fun ViewsContainer.registerBox2dSupportOnce() { + if (views.registeredBox2dSupport) return + views.registeredBox2dSupport = true + views.ktreeSerializer.registerExtension(PhysicsKTreeSerializerExtension) + views.viewExtraBuildDebugComponent.add { views, view, container -> + val physicsContainer = container.container { + } + fun physicsContainer() { + physicsContainer.removeChildren() + val body = view.body + if (body != null) { + physicsContainer.uiCollapsibleSection("Box2D Physics") { + button("Remove") { + onClick { + body.destroyBody() + view.body = null + body.view = null + physicsContainer() + } + } + uiEditableValue(body::type) + val fixture = body.m_fixtureList + if (fixture != null) { + uiEditableValue(fixture::isSensor) + uiEditableValue(fixture::friction) + uiEditableValue(fixture::density, min = 0f, clampMin = true, clampMax = false) + uiEditableValue(fixture::restitution) + } + uiEditableValue(body::linearVelocityX, min = -100f, max = 100f, clampMin = true, clampMax = false) + uiEditableValue(body::linearVelocityY, min = -100f, max = 100f, clampMin = true, clampMax = false) + uiEditableValue(body::gravityScale, min = -100f, max = 100f, clampMin = true, clampMax = false) + uiEditableValue(body::angularVelocity) + uiEditableValue(body::isSleepingAllowed) + uiEditableValue(body::isAwake) + uiEditableValue(body::isFixedRotation) + uiEditableValue(body::isBullet) + uiEditableValue(body::isActive) + } + } else { + physicsContainer.button("Add box2d physics") { + onClick { + view.registerBodyWithFixture(type = BodyType.STATIC) + views.debugSaveView("Add physics", view) + physicsContainer() + } + } + } + physicsContainer.root?.relayout() + } + physicsContainer() + } + //views.serializer.register() +} +*/ \ No newline at end of file diff --git a/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/WorldView.kt b/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/WorldView.kt new file mode 100644 index 0000000..26ad5ea --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/korlibs/korge/box2d/WorldView.kt @@ -0,0 +1,271 @@ +package korlibs.korge.box2d + +import korlibs.datastructure.* +import korlibs.time.milliseconds +import korlibs.time.seconds +import korlibs.korge.component.* +import korlibs.korge.view.* +import korlibs.korge.view.Circle +import korlibs.io.lang.* +import korlibs.io.serialization.xml.* +import korlibs.korge.view.Ellipse +import korlibs.math.geom.* +import korlibs.time.TimeSpan +import org.jbox2d.collision.shapes.* +import org.jbox2d.common.* +import org.jbox2d.dynamics.* +import org.jbox2d.userdata.* +import kotlin.native.concurrent.ThreadLocal + +@PublishedApi +internal val DEFAULT_SCALE = 20.0 +@PublishedApi +internal val DEFAULT_GRAVITY_Y = 9.8f + +@ThreadLocal +var Views.registeredBox2dSupport: Boolean by Extra.Property { false } + +fun Views.checkBox2dRegistered() { + if (!registeredBox2dSupport) error("You should call Views.registerBox2dSupport()") +} + +var World.component: Box2dWorldComponent? + get() = get(Box2dWorldComponent.Key) + set(value) { + set(Box2dWorldComponent.Key, value) + } + +class Box2dWorldComponent( + val worldView: View, + override val world: World, + var velocityIterations: Int = 6, + var positionIterations: Int = 2, + var autoDestroyBodies: Boolean = true, + val step: TimeSpan = 16.milliseconds +) : WorldRef { + var updater = worldView.addFixedUpdater(step) { + update(step) + } + + init { + world.component = this + } + + object Key : Box2dTypedUserData.Key() + + private val tempVec = Vec2() + private val tempPos = MPoint() + private fun update(step: TimeSpan) { + world.step(step.seconds.toFloat(), velocityIterations, positionIterations) + world.forEachBody { node -> + val view = node.view + + if (view != null) { + val worldScale = world.customScale + val worldScaleInv = 1.0 / worldScale + + //val viewPos = view.getPositionRelativeTo(worldView, tempPos) + val viewPos = tempPos.setTo(view.x, view.y) + + if (viewPos.x != node.viewInfo.x || viewPos.y != node.viewInfo.y || view.rotation != node.viewInfo.rotation) { + node.setTransform( + tempVec.set(viewPos.x * worldScaleInv, viewPos.y * worldScaleInv), + view.rotation + ) + if (!node.viewInfo.firstFrame) { + node.linearVelocity = tempVec.set(0f, 0f) + node.angularVelocity = 0f + node.isActive = true + node.isAwake = true + } + } + + val newX = node.position.x.toDouble() * worldScale + val newY = node.position.y.toDouble() * worldScale + + view.position(newX, newY) + //view.setPositionRelativeTo(worldView, tempPos.setTo(newX, newY)) + + view.rotation = node.angle + + val viewRoot = view.root + val viewRootStage = viewRoot is Stage + + node.viewInfo.x = view.x.toDouble() + node.viewInfo.y = view.y.toDouble() + node.viewInfo.rotation = view.rotation + node.viewInfo.firstFrame = false + + if (autoDestroyBodies && node.viewInfo.onStage && !viewRootStage) { + world.destroyBody(node) + node.view?.body = null + node.view = null + } + + node.viewInfo.onStage = viewRootStage + } + } + } +} + +@ThreadLocal +var View.box2dWorldComponent by Extra.PropertyThis { null } + +fun View.getOrCreateBox2dWorld(): Box2dWorldComponent { + if (this.box2dWorldComponent == null) { + this.box2dWorldComponent = Box2dWorldComponent(this, World(0f, DEFAULT_GRAVITY_Y).also { it.customScale = DEFAULT_SCALE }, 6, 2) + } + return this.box2dWorldComponent!! +} + +val View.nearestBox2dWorldComponent: Box2dWorldComponent + get() { + var nearestReference: View? = null + var view: View? = this + while (view != null) { + val component = view.box2dWorldComponent + if (component != null) { + return component + } + //if (view.parent == null || view is View.Reference) { + if (view is View.Reference) { + if (nearestReference == null) { + nearestReference = view + } + } + if (view.parent == null) { + return (nearestReference ?: view).getOrCreateBox2dWorld() + } + view = view.parent + } + invalidOp + } + +val View.nearestBox2dWorld: World get() = nearestBox2dWorldComponent.world + +inline fun View.createBody(world: World? = null, callback: BodyDef.() -> Unit): Body = (world ?: nearestBox2dWorld).createBody(BodyDef().apply(callback)) + +/** Shortcut to create and attach a [Fixture] to this [Body] */ +inline fun Body.fixture(callback: FixtureDef.() -> Unit): Body = this.also { createFixture(FixtureDef().apply(callback)) } + +@ThreadLocal +var View.body by Extra.PropertyThis("box2dBody") { null } + +inline fun T.registerBody(body: Body): T { + body.view = this + this.body = body + return this +} + +//private val BOX2D_BODY_KEY = "box2dBody" + +private val ViewKey = Box2dTypedUserData.Key() + +var Body.view: View? + get() = this[ViewKey] + set(value) { + this[ViewKey] = value + } + +/** Shortcut to create a simple [Body] to this [World] with the specified properties */ +inline fun T.registerBodyWithFixture( + angularVelocity: Number = 0.0, + linearVelocityX: Number = 0.0, + linearVelocityY: Number = 0.0, + linearDamping: Number = 0.0, + angularDamping: Number = 0.0, + gravityScale: Number = 1.0, + shape: Shape? = null, + allowSleep: Boolean = true, + awake: Boolean = true, + fixedRotation: Boolean = false, + bullet: Boolean = false, + type: BodyType = BodyType.STATIC, + friction: Number = 0.2, + restitution: Number = 0.2, + active: Boolean = true, + isSensor: Boolean = false, + density: Number = 1.0, + world: World? = null, +): T { + val view = this + + val body = createBody(world) { + this.type = type + this.angle = rotation + this.angularVelocity = angularVelocity.toFloat() + this.position.set(x.toFloat(), y.toFloat()) + this.linearVelocity.set(linearVelocityX.toFloat(), linearVelocityY.toFloat()) + this.linearDamping = linearDamping.toFloat() + this.angularDamping = angularDamping.toFloat() + this.gravityScale = gravityScale.toFloat() + this.allowSleep = allowSleep + this.fixedRotation = fixedRotation + this.bullet = bullet + this.awake = awake + this.active = active + } + val world = body.world + + body.fixture { + this.shape = shape ?: + when { + view is Circle -> CircleShape(view.radius / world.customScale) + view is Ellipse && view.isCircle -> CircleShape(view.radius.width / world.customScale) + else -> BoxShape(getLocalBounds() / world.customScale) + } + + //BoxShape(width / world.customScale, height / world.customScale) + this.isSensor = isSensor + this.friction = friction.toFloat() + this.restitution = restitution.toFloat() + this.density = density.toFloat() + } + //body._linearVelocity.set(linearVelocityX.toFloat(), linearVelocityY.toFloat()) + body.view = this + this.body = body + return this +} + +fun BoxShape(rect: Rectangle) = PolygonShape().apply { + count = 4 + vertices[0].set(rect.left, rect.top) + vertices[1].set(rect.right, rect.top) + vertices[2].set(rect.right, rect.bottom) + vertices[3].set(rect.left, rect.bottom) + normals[0].set(0.0f, -1.0f) + normals[1].set(1.0f, 0.0f) + normals[2].set(0.0f, 1.0f) + normals[3].set(-1.0f, 0.0f) + centroid.setZero() +} + +/** + * Creates a [PolygonShape] as a box with the specified [width] and [height] + */ +inline fun BoxShape(width: Number, height: Number) = PolygonShape().apply { + count = 4 + vertices[0].set(0, 0) + vertices[1].set(width, 0) + vertices[2].set(width, height) + vertices[3].set(0, height) + normals[0].set(0.0f, -1.0f) + normals[1].set(1.0f, 0.0f) + normals[2].set(0.0f, 1.0f) + normals[3].set(-1.0f, 0.0f) + centroid.setZero() +} + +inline fun Container.worldView( + gravityX: Number = 0.0, + gravityY: Number = DEFAULT_GRAVITY_Y.toDouble(), + velocityIterations: Int = 6, + positionIterations: Int = 2, + callback: @ViewDslMarker Container.() -> Unit = {} +): Container = container(callback = callback).also { + it.getOrCreateBox2dWorld().also { + it.world.gravity.set(gravityX, gravityY) + it.velocityIterations = velocityIterations + it.positionIterations = positionIterations + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactFilter.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactFilter.kt new file mode 100644 index 0000000..32851d4 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactFilter.kt @@ -0,0 +1,57 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:25:42 AM Jul 15, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.dynamics.Fixture + +// updated to rev 100 +/** + * Implement this class to provide collision filtering. In other words, you can implement + * this class if you want finer control over contact creation. + * @author Daniel Murphy + */ +class ContactFilter { + + /** + * Return true if contact calculations should be performed between these two shapes. + * @warning for performance reasons this is only called when the AABBs begin to overlap. + * @param fixtureA + * @param fixtureB + * @return + */ + fun shouldCollide(fixtureA: Fixture, fixtureB: Fixture): Boolean { + val filterA = fixtureA.filterData + val filterB = fixtureB.filterData + + if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) { + return filterA.groupIndex > 0 + } + + val collide = filterA.maskBits and filterB.categoryBits != 0 && filterA.categoryBits and filterB.maskBits != 0 + return collide + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactImpulse.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactImpulse.kt new file mode 100644 index 0000000..8492045 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactImpulse.kt @@ -0,0 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 3:43:53 AM Jul 7, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.common.Settings + +/** + * Contact impulses for reporting. Impulses are used instead of forces because sub-step forces may + * approach infinity for rigid body collisions. These match up one-to-one with the contact points in + * b2Manifold. + * + * @author Daniel Murphy + */ +class ContactImpulse { + var normalImpulses = FloatArray(Settings.maxManifoldPoints) + var tangentImpulses = FloatArray(Settings.maxManifoldPoints) + var count: Int = 0 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactListener.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactListener.kt new file mode 100644 index 0000000..5afe0b6 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ContactListener.kt @@ -0,0 +1,86 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.callbacks + +import org.jbox2d.collision.Manifold +import org.jbox2d.dynamics.contacts.Contact + +// updated to rev 100 +/** + * Implement this class to get contact information. You can use these results for + * things like sounds and game logic. You can also get contact results by + * traversing the contact lists after the time step. However, you might miss + * some contacts because continuous physics leads to sub-stepping. + * Additionally you may receive multiple callbacks for the same contact in a + * single time step. + * You should strive to make your callbacks efficient because there may be + * many callbacks per time step. + * @warning You cannot create/destroy Box2D entities inside these callbacks. + * @author Daniel Murphy + */ +interface ContactListener { + + /** + * Called when two fixtures begin to touch. + * @param contact + */ + fun beginContact(contact: Contact) + + /** + * Called when two fixtures cease to touch. + * @param contact + */ + fun endContact(contact: Contact) + + /** + * This is called after a contact is updated. This allows you to inspect a + * contact before it goes to the solver. If you are careful, you can modify the + * contact manifold (e.g. disable contact). + * A copy of the old manifold is provided so that you can detect changes. + * Note: this is called only for awake bodies. + * Note: this is called even when the number of contact points is zero. + * Note: this is not called for sensors. + * Note: if you set the number of contact points to zero, you will not + * get an EndContact callback. However, you may get a BeginContact callback + * the next step. + * Note: the oldManifold parameter is pooled, so it will be the same object for every callback + * for each thread. + * @param contact + * @param oldManifold + */ + fun preSolve(contact: Contact, oldManifold: Manifold) + + /** + * This lets you inspect a contact after the solver is finished. This is useful + * for inspecting impulses. + * Note: the contact manifold does not include time of impact impulses, which can be + * arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly + * in a separate data structure. + * Note: this is only called for contacts that are touching, solid, and awake. + * @param contact + * @param impulse this is usually a pooled variable, so it will be modified after + * this call + */ + fun postSolve(contact: Contact, impulse: ContactImpulse) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DebugDraw.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DebugDraw.kt new file mode 100644 index 0000000..5107d85 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DebugDraw.kt @@ -0,0 +1,276 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:35:29 AM Jul 15, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.common.Color3f +import org.jbox2d.common.IViewportTransform +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.particle.ParticleColor + +/** + * Implement this abstract class to allow JBox2d to automatically draw your physics for debugging + * purposes. Not intended to replace your own custom rendering routines! + * + * @author Daniel Murphy + */ +abstract class DebugDraw constructor(viewport: IViewportTransform? = null) { + + + var flags: Int = 0 + var viewportTranform: IViewportTransform? = null + protected set + + init { + flags = 0 + viewportTranform = viewport + } + + fun setViewportTransform(viewportTransform: IViewportTransform) { + this.viewportTranform = viewportTransform + } + + fun appendFlags(flags: Int) { + this.flags = this.flags or flags + } + + fun clearFlags(flags: Int) { + this.flags = this.flags and flags.inv() + } + + /** + * Draw a closed polygon provided in CCW order. This implementation uses + * [.drawSegment] to draw each side of the polygon. + * + * @param vertices + * @param vertexCount + * @param color + */ + fun drawPolygon(vertices: Array, vertexCount: Int, color: Color3f) { + if (vertexCount == 1) { + drawSegment(vertices[0], vertices[0], color) + return + } + + var i = 0 + while (i < vertexCount - 1) { + drawSegment(vertices[i], vertices[i + 1], color) + i += 1 + } + + if (vertexCount > 2) { + drawSegment(vertices[vertexCount - 1], vertices[0], color) + } + } + + abstract fun drawPoint(argPoint: Vec2, argRadiusOnScreen: Float, argColor: Color3f) + + /** + * Draw a solid closed polygon provided in CCW order. + * + * @param vertices + * @param vertexCount + * @param color + */ + abstract fun drawSolidPolygon(vertices: Array, vertexCount: Int, color: Color3f) + + /** + * Draw a circle. + * + * @param center + * @param radius + * @param color + */ + abstract fun drawCircle(center: Vec2, radius: Float, color: Color3f) + + /** Draws a circle with an axis */ + fun drawCircle(center: Vec2, radius: Float, axis: Vec2, color: Color3f) { + drawCircle(center, radius, color) + } + + /** + * Draw a solid circle. + * + * @param center + * @param radius + * @param axis + * @param color + */ + abstract fun drawSolidCircle(center: Vec2, radius: Float, axis: Vec2, color: Color3f) + + /** + * Draw a line segment. + * + * @param p1 + * @param p2 + * @param color + */ + abstract fun drawSegment(p1: Vec2, p2: Vec2, color: Color3f) + + /** + * Draw a transform. Choose your own length scale + * + * @param xf + */ + abstract fun drawTransform(xf: Transform) + + /** + * Draw a string. + * + * @param x + * @param y + * @param s + * @param color + */ + abstract fun drawString(x: Float, y: Float, s: String, color: Color3f) + + /** + * Draw a particle array + * + * @param colors can be null + */ + abstract fun drawParticles(centers: Array, radius: Float, colors: Array, count: Int) + + /** + * Draw a particle array + * + * @param colors can be null + */ + abstract fun drawParticlesWireframe(centers: Array, radius: Float, colors: Array, + count: Int) + + /** Called at the end of drawing a world */ + fun flush() {} + + fun drawString(pos: Vec2, s: String, color: Color3f) { + drawString(pos.x, pos.y, s, color) + } + + /** + * @param argScreen + * @param argWorld + */ + fun getScreenToWorldToOut(argScreen: Vec2, argWorld: Vec2) { + viewportTranform!!.getScreenToWorld(argScreen, argWorld) + } + + /** + * @param argWorld + * @param argScreen + */ + fun getWorldToScreenToOut(argWorld: Vec2, argScreen: Vec2) { + viewportTranform!!.getWorldToScreen(argWorld, argScreen) + } + + /** + * Takes the world coordinates and puts the corresponding screen coordinates in argScreen. + * + * @param worldX + * @param worldY + * @param argScreen + */ + fun getWorldToScreenToOut(worldX: Float, worldY: Float, argScreen: Vec2) { + argScreen.set(worldX, worldY) + viewportTranform!!.getWorldToScreen(argScreen, argScreen) + } + + /** + * takes the world coordinate (argWorld) and returns the screen coordinates. + * + * @param argWorld + */ + fun getWorldToScreen(argWorld: Vec2): Vec2 { + val screen = Vec2() + viewportTranform!!.getWorldToScreen(argWorld, screen) + return screen + } + + /** + * Takes the world coordinates and returns the screen coordinates. + * + * @param worldX + * @param worldY + */ + fun getWorldToScreen(worldX: Float, worldY: Float): Vec2 { + val argScreen = Vec2(worldX, worldY) + viewportTranform!!.getWorldToScreen(argScreen, argScreen) + return argScreen + } + + /** + * takes the screen coordinates and puts the corresponding world coordinates in argWorld. + * + * @param screenX + * @param screenY + * @param argWorld + */ + fun getScreenToWorldToOut(screenX: Float, screenY: Float, argWorld: Vec2) { + argWorld.set(screenX, screenY) + viewportTranform!!.getScreenToWorld(argWorld, argWorld) + } + + /** + * takes the screen coordinates (argScreen) and returns the world coordinates + * + * @param argScreen + */ + fun getScreenToWorld(argScreen: Vec2): Vec2 { + val world = Vec2() + viewportTranform!!.getScreenToWorld(argScreen, world) + return world + } + + /** + * takes the screen coordinates and returns the world coordinates. + * + * @param screenX + * @param screenY + */ + fun getScreenToWorld(screenX: Float, screenY: Float): Vec2 { + val screen = Vec2(screenX, screenY) + viewportTranform!!.getScreenToWorld(screen, screen) + return screen + } + + companion object { + + /** Draw shapes */ + val e_shapeBit = 1 shl 1 + /** Draw joint connections */ + val e_jointBit = 1 shl 2 + /** Draw axis aligned bounding boxes */ + val e_aabbBit = 1 shl 3 + /** Draw pairs of connected objects */ + val e_pairBit = 1 shl 4 + /** Draw center of mass frame */ + val e_centerOfMassBit = 1 shl 5 + /** Draw dynamic tree */ + val e_dynamicTreeBit = 1 shl 6 + /** Draw only the wireframe for drawing performance */ + val e_wireframeDrawingBit = 1 shl 7 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DestructionListener.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DestructionListener.kt new file mode 100644 index 0000000..8e298f8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/DestructionListener.kt @@ -0,0 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:23:30 AM Jul 15, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.dynamics.Fixture +import org.jbox2d.dynamics.joints.Joint + +/** + * Joints and fixtures are destroyed when their associated + * body is destroyed. Implement this listener so that you + * may nullify references to these joints and shapes. + * @author Daniel Murphy + */ +interface DestructionListener { + + /** + * Called when any joint is about to be destroyed due + * to the destruction of one of its attached bodies. + * @param joint + */ + fun sayGoodbye(joint: Joint) + + /** + * Called when any fixture is about to be destroyed due + * to the destruction of its parent body. + * @param fixture + */ + fun sayGoodbye(fixture: Fixture) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/PairCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/PairCallback.kt new file mode 100644 index 0000000..c8d5bfd --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/PairCallback.kt @@ -0,0 +1,29 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.callbacks + +// updated to rev 100 +interface PairCallback { + fun addPair(userDataA: Any?, userDataB: Any?) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleDestructionListener.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleDestructionListener.kt new file mode 100644 index 0000000..979208a --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleDestructionListener.kt @@ -0,0 +1,19 @@ +package org.jbox2d.callbacks + +import org.jbox2d.dynamics.World +import org.jbox2d.particle.ParticleGroup + +interface ParticleDestructionListener { + /** + * Called when any particle group is about to be destroyed. + */ + fun sayGoodbye(group: ParticleGroup) + + /** + * Called when a particle is about to be destroyed. The index can be used in conjunction with + * [World.getParticleUserDataBuffer] to determine which particle has been destroyed. + * + * @param index + */ + fun sayGoodbye(index: Int) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleQueryCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleQueryCallback.kt new file mode 100644 index 0000000..fb218c4 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleQueryCallback.kt @@ -0,0 +1,18 @@ +package org.jbox2d.callbacks + +import org.jbox2d.dynamics.World + +/** + * Callback class for AABB queries. See + * [World.queryAABB]. + * + * @author dmurph + */ +interface ParticleQueryCallback { + /** + * Called for each particle found in the query AABB. + * + * @return false to terminate the query. + */ + fun reportParticle(index: Int): Boolean +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleRaycastCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleRaycastCallback.kt new file mode 100644 index 0000000..cfb2165 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/ParticleRaycastCallback.kt @@ -0,0 +1,19 @@ +package org.jbox2d.callbacks + +import org.jbox2d.common.Vec2 + +interface ParticleRaycastCallback { + /** + * Called for each particle found in the query. See + * [RayCastCallback.reportFixture] for + * argument info. + * + * @param index + * @param point + * @param normal + * @param fraction + * @return + */ + fun reportParticle(index: Int, point: Vec2, normal: Vec2, fraction: Float): Float + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/QueryCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/QueryCallback.kt new file mode 100644 index 0000000..4385a19 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/QueryCallback.kt @@ -0,0 +1,45 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:30:03 AM Jul 15, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.dynamics.Fixture +import org.jbox2d.dynamics.World + +/** + * Callback class for AABB queries. + * See [World.queryAABB]. + * @author Daniel Murphy + */ +interface QueryCallback { + + /** + * Called for each fixture found in the query AABB. + * @param fixture + * @return false to terminate the query. + */ + fun reportFixture(fixture: Fixture): Boolean +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/RayCastCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/RayCastCallback.kt new file mode 100644 index 0000000..d558740 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/RayCastCallback.kt @@ -0,0 +1,55 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:33:10 AM Jul 15, 2010 + */ +package org.jbox2d.callbacks + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Fixture +import org.jbox2d.dynamics.World + +/** + * Callback class for ray casts. + * See [World.raycast] + * @author Daniel Murphy + */ +interface RayCastCallback { + + /** + * Called for each fixture found in the query. You control how the ray cast + * proceeds by returning a float: + * return -1: ignore this fixture and continue + * return 0: terminate the ray cast + * return fraction: clip the ray to this point + * return 1: don't clip the ray and continue + * @param fixture the fixture hit by the ray + * @param point the point of initial intersection + * @param normal the normal vector at the point of intersection + * @return -1 to filter, 0 to terminate, fraction to clip the ray for + * closest hit, 1 to continue + * @param fraction + */ + fun reportFixture(fixture: Fixture, point: Vec2, normal: Vec2, fraction: Float): Float +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeCallback.kt new file mode 100644 index 0000000..c9579ba --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeCallback.kt @@ -0,0 +1,41 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.callbacks + +import org.jbox2d.collision.broadphase.DynamicTree + +// update to rev 100 +/** + * callback for [DynamicTree] + * @author Daniel Murphy + */ +interface TreeCallback { + + /** + * Callback from a query request. + * @param proxyId the id of the proxy + * @return if the query should be continued + */ + fun treeCallback(proxyId: Int): Boolean +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeRayCastCallback.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeRayCastCallback.kt new file mode 100644 index 0000000..3edba33 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/callbacks/TreeRayCastCallback.kt @@ -0,0 +1,43 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.callbacks + +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.broadphase.DynamicTree + +// updated to rev 100 + +/** + * callback for [DynamicTree] + * @author Daniel Murphy + */ +interface TreeRayCastCallback { + /** + * + * @param input + * @param nodeId + * @return the fraction to the node + */ + fun raycastCallback(input: RayCastInput, nodeId: Int): Float +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/AABB.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/AABB.kt new file mode 100644 index 0000000..36bdd6d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/AABB.kt @@ -0,0 +1,320 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.pooling.IWorldPool +import org.jbox2d.pooling.normal.DefaultWorldPool + +/** An axis-aligned bounding box. */ +class AABB { + /** Bottom left vertex of bounding box. */ + + val lowerBound: Vec2 + /** Top right vertex of bounding box. */ + + val upperBound: Vec2 + + /** Verify that the bounds are sorted */ + val isValid: Boolean + get() { + val dx = upperBound.x - lowerBound.x + if (dx < 0f) { + return false + } + val dy = upperBound.y - lowerBound.y + return if (dy < 0) { + false + } else lowerBound.isValid && upperBound.isValid + } + + /** + * Get the center of the AABB + * + * @return + */ + val center: Vec2 + get() { + val center = Vec2(lowerBound) + center.addLocal(upperBound) + center.mulLocal(.5f) + return center + } + + /** + * Get the extents of the AABB (half-widths). + * + * @return + */ + val extents: Vec2 + get() { + val center = Vec2(upperBound) + center.subLocal(lowerBound) + center.mulLocal(.5f) + return center + } + + /** + * Gets the perimeter length + * + * @return + */ + val perimeter: Float + get() = 2.0f * (upperBound.x - lowerBound.x + upperBound.y - lowerBound.y) + + /** + * Creates the default object, with vertices at 0,0 and 0,0. + */ + constructor() { + lowerBound = Vec2() + upperBound = Vec2() + } + + /** + * Copies from the given object + * + * @param copy the object to copy from + */ + constructor(copy: AABB) : this(copy.lowerBound, copy.upperBound) {} + + /** + * Creates an AABB object using the given bounding vertices. + * + * @param lowerVertex the bottom left vertex of the bounding box + * @param maxVertex the top right vertex of the bounding box + */ + constructor(lowerVertex: Vec2, upperVertex: Vec2) { + this.lowerBound = lowerVertex.clone() // clone to be safe + this.upperBound = upperVertex.clone() + } + + /** + * Sets this object from the given object + * + * @param aabb the object to copy from + */ + fun set(aabb: AABB) { + val v = aabb.lowerBound + lowerBound.x = v.x + lowerBound.y = v.y + val v1 = aabb.upperBound + upperBound.x = v1.x + upperBound.y = v1.y + } + + fun getCenterToOut(out: Vec2) { + out.x = (lowerBound.x + upperBound.x) * .5f + out.y = (lowerBound.y + upperBound.y) * .5f + } + + fun getExtentsToOut(out: Vec2) { + out.x = (upperBound.x - lowerBound.x) * .5f + out.y = (upperBound.y - lowerBound.y) * .5f // thanks FDN1 + } + + fun getVertices(argRay: Array) { + argRay[0].set(lowerBound) + argRay[1].set(lowerBound) + argRay[1].x += upperBound.x - lowerBound.x + argRay[2].set(upperBound) + argRay[3].set(upperBound) + argRay[3].x -= upperBound.x - lowerBound.x + } + + /** + * Combine two AABBs into this one. + * + * @param aabb1 + * @param aab + */ + fun combine(aabb1: AABB, aab: AABB) { + lowerBound.x = if (aabb1.lowerBound.x < aab.lowerBound.x) aabb1.lowerBound.x else aab.lowerBound.x + lowerBound.y = if (aabb1.lowerBound.y < aab.lowerBound.y) aabb1.lowerBound.y else aab.lowerBound.y + upperBound.x = if (aabb1.upperBound.x > aab.upperBound.x) aabb1.upperBound.x else aab.upperBound.x + upperBound.y = if (aabb1.upperBound.y > aab.upperBound.y) aabb1.upperBound.y else aab.upperBound.y + } + + /** + * Combines another aabb with this one + * + * @param aabb + */ + fun combine(aabb: AABB) { + lowerBound.x = if (lowerBound.x < aabb.lowerBound.x) lowerBound.x else aabb.lowerBound.x + lowerBound.y = if (lowerBound.y < aabb.lowerBound.y) lowerBound.y else aabb.lowerBound.y + upperBound.x = if (upperBound.x > aabb.upperBound.x) upperBound.x else aabb.upperBound.x + upperBound.y = if (upperBound.y > aabb.upperBound.y) upperBound.y else aabb.upperBound.y + } + + /** + * Does this aabb contain the provided AABB. + * + * @return + */ + operator fun contains(aabb: AABB): Boolean { + /* + * boolean result = true; result = result && lowerBound.x <= aabb.lowerBound.x; result = result + * && lowerBound.y <= aabb.lowerBound.y; result = result && aabb.upperBound.x <= upperBound.x; + * result = result && aabb.upperBound.y <= upperBound.y; return result; + */ + // djm: faster putting all of them together, as if one is false we leave the logic + // early + return (lowerBound.x <= aabb.lowerBound.x && lowerBound.y <= aabb.lowerBound.y + && aabb.upperBound.x <= upperBound.x && aabb.upperBound.y <= upperBound.y) + } + + /** + * From Real-time Collision Detection, p179. + * + * @param output + * @param input + */ + + fun raycast(output: RayCastOutput, input: RayCastInput, + argPool: IWorldPool = DefaultWorldPool(4, 4)): Boolean { + var tmin = -Float.MAX_VALUE + var tmax = Float.MAX_VALUE + + val p = argPool.popVec2() + val d = argPool.popVec2() + val absD = argPool.popVec2() + val normal = argPool.popVec2() + + p.set(input.p1) + d.set(input.p2).subLocal(input.p1) + Vec2.absToOut(d, absD) + + // x then y + if (absD.x < Settings.EPSILON) { + // Parallel. + if (p.x < lowerBound.x || upperBound.x < p.x) { + argPool.pushVec2(4) + return false + } + } else { + val inv_d = 1.0f / d.x + var t1 = (lowerBound.x - p.x) * inv_d + var t2 = (upperBound.x - p.x) * inv_d + + // Sign of the normal vector. + var s = -1.0f + + if (t1 > t2) { + val temp = t1 + t1 = t2 + t2 = temp + s = 1.0f + } + + // Push the min up + if (t1 > tmin) { + normal.setZero() + normal.x = s + tmin = t1 + } + + // Pull the max down + tmax = MathUtils.min(tmax, t2) + + if (tmin > tmax) { + argPool.pushVec2(4) + return false + } + } + + if (absD.y < Settings.EPSILON) { + // Parallel. + if (p.y < lowerBound.y || upperBound.y < p.y) { + argPool.pushVec2(4) + return false + } + } else { + val inv_d = 1.0f / d.y + var t1 = (lowerBound.y - p.y) * inv_d + var t2 = (upperBound.y - p.y) * inv_d + + // Sign of the normal vector. + var s = -1.0f + + if (t1 > t2) { + val temp = t1 + t1 = t2 + t2 = temp + s = 1.0f + } + + // Push the min up + if (t1 > tmin) { + normal.setZero() + normal.y = s + tmin = t1 + } + + // Pull the max down + tmax = MathUtils.min(tmax, t2) + + if (tmin > tmax) { + argPool.pushVec2(4) + return false + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (tmin < 0.0f || input.maxFraction < tmin) { + argPool.pushVec2(4) + return false + } + + // Intersection. + output.fraction = tmin + output.normal.x = normal.x + output.normal.y = normal.y + argPool.pushVec2(4) + return true + } + + override fun toString(): String { + val s = "AABB[$lowerBound . $upperBound]" + return s + } + + companion object { + fun testOverlap(a: AABB, b: AABB): Boolean { + if (b.lowerBound.x - a.upperBound.x > 0.0f || b.lowerBound.y - a.upperBound.y > 0.0f) { + return false + } + + return !(a.lowerBound.x - b.upperBound.x > 0.0f || a.lowerBound.y - b.upperBound.y > 0.0f) + + } + } +} +/** + * @param output + * @param input + * @return + */ diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Collision.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Collision.kt new file mode 100644 index 0000000..66d3469 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Collision.kt @@ -0,0 +1,1429 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.collision.Distance.SimplexCache +import org.jbox2d.collision.Manifold.ManifoldType +import org.jbox2d.collision.shapes.CircleShape +import org.jbox2d.collision.shapes.EdgeShape +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.collision.shapes.Shape +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.* +import org.jbox2d.pooling.IWorldPool + +/** + * Functions used for computing contact points, distance queries, and TOI queries. Collision methods + * are non-static for pooling speed, retrieve a collision object from the [SingletonPool]. + * Should not be finalructed. + * + * @author Daniel Murphy + */ +class Collision(private val pool: IWorldPool) { + + private val input = DistanceInput() + private val cache = SimplexCache() + private val output = DistanceOutput() + + // djm pooling, and from above + private val temp = Vec2() + private val xf = Transform() + private val n = Vec2() + private val v1 = Vec2() + + private val results1 = EdgeResults() + private val results2 = EdgeResults() + private val incidentEdge = Array(2) { ClipVertex() } + private val localTangent = Vec2() + private val localNormal = Vec2() + private val planePoint = Vec2() + private val tangent = Vec2() + private val v11 = Vec2() + private val v12 = Vec2() + private val clipPoints1 = Array(2) { ClipVertex() } + private val clipPoints2 = Array(2) { ClipVertex() } + + private val Q = Vec2() + private val e = Vec2() + private val cf = ContactID() + private val e1 = Vec2() + private val P = Vec2() + + private val collider = EPCollider() + + /** + * Determine if two generic shapes overlap. + * + * @param shapeA + * @param shapeB + * @param xfA + * @param xfB + * @return + */ + fun testOverlap(shapeA: Shape, indexA: Int, shapeB: Shape, indexB: Int, + xfA: Transform, xfB: Transform): Boolean { + input.proxyA.set(shapeA, indexA) + input.proxyB.set(shapeB, indexB) + input.transformA.set(xfA) + input.transformB.set(xfB) + input.useRadii = true + + cache.count = 0 + + pool.distance.distance(output, cache, input) + // djm note: anything significant about 10.0f? + return output.distance < 10.0f * Settings.EPSILON + } + + /** + * Compute the collision manifold between two circles. + * + * @param manifold + * @param circle1 + * @param xfA + * @param circle2 + * @param xfB + */ + fun collideCircles(manifold: Manifold, circle1: CircleShape, + xfA: Transform, circle2: CircleShape, xfB: Transform) { + manifold.pointCount = 0 + // before inline: + // Transform.mulToOut(xfA, circle1.m_p, pA); + // Transform.mulToOut(xfB, circle2.m_p, pB); + // d.set(pB).subLocal(pA); + // float distSqr = d.x * d.x + d.y * d.y; + + // after inline: + val circle1p = circle1.p + val circle2p = circle2.p + val pAx = xfA.q.c * circle1p.x - xfA.q.s * circle1p.y + xfA.p.x + val pAy = xfA.q.s * circle1p.x + xfA.q.c * circle1p.y + xfA.p.y + val pBx = xfB.q.c * circle2p.x - xfB.q.s * circle2p.y + xfB.p.x + val pBy = xfB.q.s * circle2p.x + xfB.q.c * circle2p.y + xfB.p.y + val dx = pBx - pAx + val dy = pBy - pAy + val distSqr = dx * dx + dy * dy + // end inline + + val radius = circle1.radius + circle2.radius + if (distSqr > radius * radius) { + return + } + + manifold.type = ManifoldType.CIRCLES + manifold.localPoint.set(circle1p) + manifold.localNormal.setZero() + manifold.pointCount = 1 + + manifold.points[0].localPoint.set(circle2p) + manifold.points[0].id.zero() + } + + // djm pooling, and from above + + /** + * Compute the collision manifold between a polygon and a circle. + * + * @param manifold + * @param polygon + * @param xfA + * @param circle + * @param xfB + */ + fun collidePolygonAndCircle(manifold: Manifold, polygon: PolygonShape, + xfA: Transform, circle: CircleShape, xfB: Transform) { + manifold.pointCount = 0 + // Vec2 v = circle.m_p; + + // Compute circle position in the frame of the polygon. + // before inline: + // Transform.mulToOutUnsafe(xfB, circle.m_p, c); + // Transform.mulTransToOut(xfA, c, cLocal); + // final float cLocalx = cLocal.x; + // final float cLocaly = cLocal.y; + // after inline: + val circlep = circle.p + val xfBq = xfB.q + val xfAq = xfA.q + val cx = xfBq.c * circlep.x - xfBq.s * circlep.y + xfB.p.x + val cy = xfBq.s * circlep.x + xfBq.c * circlep.y + xfB.p.y + val px = cx - xfA.p.x + val py = cy - xfA.p.y + val cLocalx = xfAq.c * px + xfAq.s * py + val cLocaly = -xfAq.s * px + xfAq.c * py + // end inline + + // Find the min separating edge. + var normalIndex = 0 + var separation = -Float.MAX_VALUE + val radius = polygon.radius + circle.radius + val vertexCount = polygon.count + var s: Float + val vertices = polygon.vertices + val normals = polygon.normals + + for (i in 0 until vertexCount) { + // before inline + // temp.set(cLocal).subLocal(vertices[i]); + // float s = Vec2.dot(normals[i], temp); + // after inline + val vertex = vertices[i] + val tempx = cLocalx - vertex.x + val tempy = cLocaly - vertex.y + s = normals[i].x * tempx + normals[i].y * tempy + + + if (s > radius) { + // early out + return + } + + if (s > separation) { + separation = s + normalIndex = i + } + } + + // Vertices that subtend the incident face. + val vertIndex1 = normalIndex + val vertIndex2 = if (vertIndex1 + 1 < vertexCount) vertIndex1 + 1 else 0 + val v1 = vertices[vertIndex1] + val v2 = vertices[vertIndex2] + + // If the center is inside the polygon ... + if (separation < Settings.EPSILON) { + manifold.pointCount = 1 + manifold.type = ManifoldType.FACE_A + + // before inline: + // manifold.localNormal.set(normals[normalIndex]); + // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f); + // manifold.points[0].localPoint.set(circle.m_p); + // after inline: + val normal = normals[normalIndex] + manifold.localNormal.x = normal.x + manifold.localNormal.y = normal.y + manifold.localPoint.x = (v1.x + v2.x) * .5f + manifold.localPoint.y = (v1.y + v2.y) * .5f + val mpoint = manifold.points[0] + mpoint.localPoint.x = circlep.x + mpoint.localPoint.y = circlep.y + mpoint.id.zero() + // end inline + + return + } + + // Compute barycentric coordinates + // before inline: + // temp.set(cLocal).subLocal(v1); + // temp2.set(v2).subLocal(v1); + // float u1 = Vec2.dot(temp, temp2); + // temp.set(cLocal).subLocal(v2); + // temp2.set(v1).subLocal(v2); + // float u2 = Vec2.dot(temp, temp2); + // after inline: + val tempX = cLocalx - v1.x + val tempY = cLocaly - v1.y + val temp2X = v2.x - v1.x + val temp2Y = v2.y - v1.y + val u1 = tempX * temp2X + tempY * temp2Y + + val temp3X = cLocalx - v2.x + val temp3Y = cLocaly - v2.y + val temp4X = v1.x - v2.x + val temp4Y = v1.y - v2.y + val u2 = temp3X * temp4X + temp3Y * temp4Y + // end inline + + if (u1 <= 0f) { + // inlined + val dx = cLocalx - v1.x + val dy = cLocaly - v1.y + if (dx * dx + dy * dy > radius * radius) { + return + } + + manifold.pointCount = 1 + manifold.type = ManifoldType.FACE_A + // before inline: + // manifold.localNormal.set(cLocal).subLocal(v1); + // after inline: + manifold.localNormal.x = cLocalx - v1.x + manifold.localNormal.y = cLocaly - v1.y + // end inline + manifold.localNormal.normalize() + manifold.localPoint.set(v1) + manifold.points[0].localPoint.set(circlep) + manifold.points[0].id.zero() + } else if (u2 <= 0.0f) { + // inlined + val dx = cLocalx - v2.x + val dy = cLocaly - v2.y + if (dx * dx + dy * dy > radius * radius) { + return + } + + manifold.pointCount = 1 + manifold.type = ManifoldType.FACE_A + // before inline: + // manifold.localNormal.set(cLocal).subLocal(v2); + // after inline: + manifold.localNormal.x = cLocalx - v2.x + manifold.localNormal.y = cLocaly - v2.y + // end inline + manifold.localNormal.normalize() + manifold.localPoint.set(v2) + manifold.points[0].localPoint.set(circlep) + manifold.points[0].id.zero() + } else { + // Vec2 faceCenter = 0.5f * (v1 + v2); + // (temp is faceCenter) + // before inline: + // temp.set(v1).addLocal(v2).mulLocal(.5f); + // + // temp2.set(cLocal).subLocal(temp); + // separation = Vec2.dot(temp2, normals[vertIndex1]); + // if (separation > radius) { + // return; + // } + // after inline: + val fcx = (v1.x + v2.x) * .5f + val fcy = (v1.y + v2.y) * .5f + + val tx = cLocalx - fcx + val ty = cLocaly - fcy + val normal = normals[vertIndex1] + separation = tx * normal.x + ty * normal.y + if (separation > radius) { + return + } + // end inline + + manifold.pointCount = 1 + manifold.type = ManifoldType.FACE_A + manifold.localNormal.set(normals[vertIndex1]) + manifold.localPoint.x = fcx // (faceCenter) + manifold.localPoint.y = fcy + manifold.points[0].localPoint.set(circlep) + manifold.points[0].id.zero() + } + } + + private val poolVec2 = Vec2() + + /** + * Find the max separation between poly1 and poly2 using edge normals from poly1. + * + * @param edgeIndex + * @param poly1 + * @param xf1 + * @param poly2 + * @param xf2 + * @return + */ + fun findMaxSeparation(results: EdgeResults, poly1: PolygonShape, + xf1: Transform, poly2: PolygonShape, xf2: Transform) { + val count1 = poly1.count + val count2 = poly2.count + val n1s = poly1.normals + val v1s = poly1.vertices + val v2s = poly2.vertices + + Transform.mulTransToOutUnsafe(xf2, xf1, xf, poolVec2) + val xfq = xf.q + + var bestIndex = 0 + var maxSeparation = -Float.MAX_VALUE + for (i in 0 until count1) { + // Get poly1 normal in frame2. + Rot.mulToOutUnsafe(xfq, n1s[i], n) + Transform.mulToOutUnsafe(xf, v1s[i], v1) + + // Find deepest point for normal i. + var si = Float.MAX_VALUE + for (j in 0 until count2) { + val v2sj = v2s[j] + val sij = n.x * (v2sj.x - v1.x) + n.y * (v2sj.y - v1.y) + if (sij < si) { + si = sij + } + } + + if (si > maxSeparation) { + maxSeparation = si + bestIndex = i + } + } + + results.edgeIndex = bestIndex + results.separation = maxSeparation + } + + fun findIncidentEdge(c: Array, poly1: PolygonShape, + xf1: Transform, edge1: Int, poly2: PolygonShape, xf2: Transform) { + val count1 = poly1.count + val normals1 = poly1.normals + + val count2 = poly2.count + val vertices2 = poly2.vertices + val normals2 = poly2.normals + + assert(0 <= edge1 && edge1 < count1) + + val c0 = c[0] + val c1 = c[1] + val xf1q = xf1.q + val xf2q = xf2.q + + // Get the normal of the reference edge in poly2's frame. + // Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1])); + // before inline: + // Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary + // Rot.mulTrans(xf2.q, normal1, normal1); + // after inline: + val v = normals1[edge1] + val tempx = xf1q.c * v.x - xf1q.s * v.y + val tempy = xf1q.s * v.x + xf1q.c * v.y + val normal1x = xf2q.c * tempx + xf2q.s * tempy + val normal1y = -xf2q.s * tempx + xf2q.c * tempy + + // end inline + + // Find the incident edge on poly2. + var index = 0 + var minDot = Float.MAX_VALUE + for (i in 0 until count2) { + val b = normals2[i] + val dot = normal1x * b.x + normal1y * b.y + if (dot < minDot) { + minDot = dot + index = i + } + } + + // Build the clip vertices for the incident edge. + val i1 = index + val i2 = if (i1 + 1 < count2) i1 + 1 else 0 + + // c0.v = Mul(xf2, vertices2[i1]); + val v1 = vertices2[i1] + val out = c0.v + out.x = xf2q.c * v1.x - xf2q.s * v1.y + xf2.p.x + out.y = xf2q.s * v1.x + xf2q.c * v1.y + xf2.p.y + c0.id.indexA = edge1.toByte() + c0.id.indexB = i1.toByte() + c0.id.typeA = ContactID.Type.FACE.ordinal.toByte() + c0.id.typeB = ContactID.Type.VERTEX.ordinal.toByte() + + // c1.v = Mul(xf2, vertices2[i2]); + val v2 = vertices2[i2] + val out1 = c1.v + out1.x = xf2q.c * v2.x - xf2q.s * v2.y + xf2.p.x + out1.y = xf2q.s * v2.x + xf2q.c * v2.y + xf2.p.y + c1.id.indexA = edge1.toByte() + c1.id.indexB = i2.toByte() + c1.id.typeA = ContactID.Type.FACE.ordinal.toByte() + c1.id.typeB = ContactID.Type.VERTEX.ordinal.toByte() + } + + /** + * Compute the collision manifold between two polygons. + * + * @param manifold + * @param polygon1 + * @param xf1 + * @param polygon2 + * @param xf2 + */ + fun collidePolygons(manifold: Manifold, polyA: PolygonShape, + xfA: Transform, polyB: PolygonShape, xfB: Transform) { + // Find edge normal of max separation on A - return if separating axis is found + // Find edge normal of max separation on B - return if separation axis is found + // Choose reference edge as min(minA, minB) + // Find incident edge + // Clip + + // The normal points from 1 to 2 + + manifold.pointCount = 0 + val totalRadius = polyA.radius + polyB.radius + + findMaxSeparation(results1, polyA, xfA, polyB, xfB) + if (results1.separation > totalRadius) { + return + } + + findMaxSeparation(results2, polyB, xfB, polyA, xfA) + if (results2.separation > totalRadius) { + return + } + + val poly1: PolygonShape // reference polygon + val poly2: PolygonShape // incident polygon + val xf1: Transform + val xf2: Transform + val edge1: Int // reference edge + val flip: Boolean + val k_tol = 0.1f * Settings.linearSlop + + if (results2.separation > results1.separation + k_tol) { + poly1 = polyB + poly2 = polyA + xf1 = xfB + xf2 = xfA + edge1 = results2.edgeIndex + manifold.type = ManifoldType.FACE_B + flip = true + } else { + poly1 = polyA + poly2 = polyB + xf1 = xfA + xf2 = xfB + edge1 = results1.edgeIndex + manifold.type = ManifoldType.FACE_A + flip = false + } + val xf1q = xf1.q + + findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2) + + val count1 = poly1.count + val vertices1 = poly1.vertices + + val iv1 = edge1 + val iv2 = if (edge1 + 1 < count1) edge1 + 1 else 0 + v11.set(vertices1[iv1]) + v12.set(vertices1[iv2]) + localTangent.x = v12.x - v11.x + localTangent.y = v12.y - v11.y + localTangent.normalize() + + // Vec2 localNormal = Vec2.cross(dv, 1.0f); + localNormal.x = 1f * localTangent.y + localNormal.y = -1f * localTangent.x + + // Vec2 planePoint = 0.5f * (v11+ v12); + planePoint.x = (v11.x + v12.x) * .5f + planePoint.y = (v11.y + v12.y) * .5f + + // Rot.mulToOutUnsafe(xf1.q, localTangent, tangent); + tangent.x = xf1q.c * localTangent.x - xf1q.s * localTangent.y + tangent.y = xf1q.s * localTangent.x + xf1q.c * localTangent.y + + // Vec2.crossToOutUnsafe(tangent, 1f, normal); + val normalx = 1f * tangent.y + val normaly = -1f * tangent.x + + + Transform.mulToOut(xf1, v11, v11) + Transform.mulToOut(xf1, v12, v12) + // v11 = Mul(xf1, v11); + // v12 = Mul(xf1, v12); + + // Face offset + // float frontOffset = Vec2.dot(normal, v11); + val frontOffset = normalx * v11.x + normaly * v11.y + + // Side offsets, extended by polytope skin thickness. + // float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius; + // float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius; + val sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius + val sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius + + // Clip incident edge against extruded edge1 side edges. + // ClipVertex clipPoints1[2]; + // ClipVertex clipPoints2[2]; + var np: Int + + // Clip to box side 1 + // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); + tangent.negateLocal() + np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1) + tangent.negateLocal() + + if (np < 2) { + return + } + + // Clip to negative box side 1 + np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2) + + if (np < 2) { + return + } + + // Now clipPoints2 contains the clipped points. + manifold.localNormal.set(localNormal) + manifold.localPoint.set(planePoint) + + var pointCount = 0 + for (i in 0 until Settings.maxManifoldPoints) { + // float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset; + val separation = normalx * clipPoints2[i].v.x + normaly * clipPoints2[i].v.y - frontOffset + + if (separation <= totalRadius) { + val cp = manifold.points[pointCount] + // cp.m_localPoint = MulT(xf2, clipPoints2[i].v); + val out = cp.localPoint + val px = clipPoints2[i].v.x - xf2.p.x + val py = clipPoints2[i].v.y - xf2.p.y + out.x = xf2.q.c * px + xf2.q.s * py + out.y = -xf2.q.s * px + xf2.q.c * py + cp.id.set(clipPoints2[i].id) + if (flip) { + // Swap features + cp.id.flip() + } + ++pointCount + } + } + + manifold.pointCount = pointCount + } + + // Compute contact points for edge versus circle. + // This accounts for edge connectivity. + fun collideEdgeAndCircle(manifold: Manifold, edgeA: EdgeShape, xfA: Transform, + circleB: CircleShape, xfB: Transform) { + manifold.pointCount = 0 + + + // Compute circle in frame of edge + // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p)); + Transform.mulToOutUnsafe(xfB, circleB.p, temp) + Transform.mulTransToOutUnsafe(xfA, temp, Q) + + val A = edgeA.vertex1 + val B = edgeA.vertex2 + e.set(B).subLocal(A) + + // Barycentric coordinates + val u = Vec2.dot(e, temp.set(B).subLocal(Q)) + val v = Vec2.dot(e, temp.set(Q).subLocal(A)) + + val radius = edgeA.radius + circleB.radius + + // ContactFeature cf; + cf.indexB = 0 + cf.typeB = ContactID.Type.VERTEX.ordinal.toByte() + + // Region A + if (v <= 0.0f) { + val P = A + d.set(Q).subLocal(P) + val dd = Vec2.dot(d, d) + if (dd > radius * radius) { + return + } + + // Is there an edge connected to A? + if (edgeA.hasVertex0) { + val A1 = edgeA.vertex0 + val B1 = A + e1.set(B1).subLocal(A1) + val u1 = Vec2.dot(e1, temp.set(B1).subLocal(Q)) + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) { + return + } + } + + cf.indexA = 0 + cf.typeA = ContactID.Type.VERTEX.ordinal.toByte() + manifold.pointCount = 1 + manifold.type = Manifold.ManifoldType.CIRCLES + manifold.localNormal.setZero() + manifold.localPoint.set(P) + // manifold.points[0].id.key = 0; + manifold.points[0].id.set(cf) + manifold.points[0].localPoint.set(circleB.p) + return + } + + // Region B + if (u <= 0.0f) { + val P = B + d.set(Q).subLocal(P) + val dd = Vec2.dot(d, d) + if (dd > radius * radius) { + return + } + + // Is there an edge connected to B? + if (edgeA.hasVertex3) { + val B2 = edgeA.vertex3 + val A2 = B + val e2 = e1 + e2.set(B2).subLocal(A2) + val v2 = Vec2.dot(e2, temp.set(Q).subLocal(A2)) + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) { + return + } + } + + cf.indexA = 1 + cf.typeA = ContactID.Type.VERTEX.ordinal.toByte() + manifold.pointCount = 1 + manifold.type = Manifold.ManifoldType.CIRCLES + manifold.localNormal.setZero() + manifold.localPoint.set(P) + // manifold.points[0].id.key = 0; + manifold.points[0].id.set(cf) + manifold.points[0].localPoint.set(circleB.p) + return + } + + // Region AB + val den = Vec2.dot(e, e) + assert(den > 0.0f) + + // Vec2 P = (1.0f / den) * (u * A + v * B); + P.set(A).mulLocal(u).addLocal(temp.set(B).mulLocal(v)) + P.mulLocal(1.0f / den) + d.set(Q).subLocal(P) + val dd = Vec2.dot(d, d) + if (dd > radius * radius) { + return + } + + n.x = -e.y + n.y = e.x + if (Vec2.dot(n, temp.set(Q).subLocal(A)) < 0.0f) { + n.set(-n.x, -n.y) + } + n.normalize() + + cf.indexA = 0 + cf.typeA = ContactID.Type.FACE.ordinal.toByte() + manifold.pointCount = 1 + manifold.type = Manifold.ManifoldType.FACE_A + manifold.localNormal.set(n) + manifold.localPoint.set(A) + // manifold.points[0].id.key = 0; + manifold.points[0].id.set(cf) + manifold.points[0].localPoint.set(circleB.p) + } + + fun collideEdgeAndPolygon(manifold: Manifold, edgeA: EdgeShape, xfA: Transform, + polygonB: PolygonShape, xfB: Transform) { + collider.collide(manifold, edgeA, xfA, polygonB, xfB) + } + + + /** + * Java-specific class for returning edge results + */ + class EdgeResults { + var separation: Float = 0.toFloat() + var edgeIndex: Int = 0 + } + + /** + * Used for computing contact manifolds. + */ + class ClipVertex { + val v: Vec2 = Vec2() + val id: ContactID = ContactID() + + fun set(cv: ClipVertex) { + val v1 = cv.v + v.x = v1.x + v.y = v1.y + val c = cv.id + id.indexA = c.indexA + id.indexB = c.indexB + id.typeA = c.typeA + id.typeB = c.typeB + } + } + + /** + * This is used for determining the state of contact points. + * + * @author Daniel Murphy + */ + enum class PointState { + /** + * point does not exist + */ + NULL_STATE, + /** + * point was added in the update + */ + ADD_STATE, + /** + * point persisted across the update + */ + PERSIST_STATE, + /** + * point was removed in the update + */ + REMOVE_STATE + } + + /** + * This structure is used to keep track of the best separating axis. + */ + internal class EPAxis { + + var type: Type? = null + var index: Int = 0 + var separation: Float = 0.toFloat() + + internal enum class Type { + UNKNOWN, EDGE_A, EDGE_B + } + } + + /** + * This holds polygon B expressed in frame A. + */ + internal class TempPolygon { + val vertices = Array(Settings.maxPolygonVertices) { Vec2() } + val normals = Array(Settings.maxPolygonVertices) { Vec2() } + var count: Int = 0 + } + + /** + * Reference face used for clipping + */ + internal class ReferenceFace { + var i1: Int = 0 + var i2: Int = 0 + val v1 = Vec2() + val v2 = Vec2() + val normal = Vec2() + + val sideNormal1 = Vec2() + var sideOffset1: Float = 0.toFloat() + + val sideNormal2 = Vec2() + var sideOffset2: Float = 0.toFloat() + } + + /** + * This class collides and edge and a polygon, taking into account edge adjacency. + */ + internal class EPCollider { + + val m_polygonB = TempPolygon() + + val m_xf = Transform() + val m_centroidB = Vec2() + var m_v0 = Vec2() + var m_v1 = Vec2() + var m_v2 = Vec2() + var m_v3 = Vec2() + val m_normal0 = Vec2() + val m_normal1 = Vec2() + val m_normal2 = Vec2() + val m_normal = Vec2() + + var m_type1: VertexType? = null + var m_type2: VertexType? = null + + val m_lowerLimit = Vec2() + val m_upperLimit = Vec2() + var m_radius: Float = 0.toFloat() + var m_front: Boolean = false + + private val edge1 = Vec2() + private val temp = Vec2() + private val edge0 = Vec2() + private val edge2 = Vec2() + private val ie = Array(2) { ClipVertex() } + private val clipPoints1 = Array(2) { ClipVertex() } + private val clipPoints2 = Array(2) { ClipVertex() } + private val rf = ReferenceFace() + private val edgeAxis = EPAxis() + private val polygonAxis = EPAxis() + + private val perp = Vec2() + private val n = Vec2() + + internal enum class VertexType { + ISOLATED, CONCAVE, CONVEX + } + + private val poolVec2 = Vec2() + + fun collide(manifold: Manifold, edgeA: EdgeShape, xfA: Transform, + polygonB: PolygonShape, xfB: Transform) { + + Transform.mulTransToOutUnsafe(xfA, xfB, m_xf, poolVec2) + Transform.mulToOutUnsafe(m_xf, polygonB.centroid, m_centroidB) + + m_v0 = edgeA.vertex0 + m_v1 = edgeA.vertex1 + m_v2 = edgeA.vertex2 + m_v3 = edgeA.vertex3 + + val hasVertex0 = edgeA.hasVertex0 + val hasVertex3 = edgeA.hasVertex3 + + edge1.set(m_v2).subLocal(m_v1) + edge1.normalize() + m_normal1.set(edge1.y, -edge1.x) + val offset1 = Vec2.dot(m_normal1, temp.set(m_centroidB).subLocal(m_v1)) + var offset0 = 0.0f + var offset2 = 0.0f + var convex1 = false + var convex2 = false + + // Is there a preceding edge? + if (hasVertex0) { + edge0.set(m_v1).subLocal(m_v0) + edge0.normalize() + m_normal0.set(edge0.y, -edge0.x) + convex1 = Vec2.cross(edge0, edge1) >= 0.0f + offset0 = Vec2.dot(m_normal0, temp.set(m_centroidB).subLocal(m_v0)) + } + + // Is there a following edge? + if (hasVertex3) { + edge2.set(m_v3).subLocal(m_v2) + edge2.normalize() + m_normal2.set(edge2.y, -edge2.x) + convex2 = Vec2.cross(edge1, edge2) > 0.0f + offset2 = Vec2.dot(m_normal2, temp.set(m_centroidB).subLocal(m_v2)) + } + + // Determine front or back collision. Determine collision normal limits. + if (hasVertex0 && hasVertex3) { + if (convex1 && convex2) { + m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal0.x + m_lowerLimit.y = m_normal0.y + m_upperLimit.x = m_normal2.x + m_upperLimit.y = m_normal2.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } + } else if (convex1) { + m_front = offset0 >= 0.0f || offset1 >= 0.0f && offset2 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal0.x + m_lowerLimit.y = m_normal0.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal2.x + m_lowerLimit.y = -m_normal2.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } + } else if (convex2) { + m_front = offset2 >= 0.0f || offset0 >= 0.0f && offset1 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = m_normal2.x + m_upperLimit.y = m_normal2.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = -m_normal0.x + m_upperLimit.y = -m_normal0.y + } + } else { + m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal2.x + m_lowerLimit.y = -m_normal2.y + m_upperLimit.x = -m_normal0.x + m_upperLimit.y = -m_normal0.y + } + } + } else if (hasVertex0) { + if (convex1) { + m_front = offset0 >= 0.0f || offset1 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal0.x + m_lowerLimit.y = m_normal0.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } + } else { + m_front = offset0 >= 0.0f && offset1 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = -m_normal0.x + m_upperLimit.y = -m_normal0.y + } + } + } else if (hasVertex3) { + if (convex2) { + m_front = offset1 >= 0.0f || offset2 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = m_normal2.x + m_upperLimit.y = m_normal2.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } + } else { + m_front = offset1 >= 0.0f && offset2 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = -m_normal2.x + m_lowerLimit.y = -m_normal2.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } + } + } else { + m_front = offset1 >= 0.0f + if (m_front) { + m_normal.x = m_normal1.x + m_normal.y = m_normal1.y + m_lowerLimit.x = -m_normal1.x + m_lowerLimit.y = -m_normal1.y + m_upperLimit.x = -m_normal1.x + m_upperLimit.y = -m_normal1.y + } else { + m_normal.x = -m_normal1.x + m_normal.y = -m_normal1.y + m_lowerLimit.x = m_normal1.x + m_lowerLimit.y = m_normal1.y + m_upperLimit.x = m_normal1.x + m_upperLimit.y = m_normal1.y + } + } + + // Get polygonB in frameA + m_polygonB.count = polygonB.count + for (i in 0 until polygonB.count) { + Transform.mulToOutUnsafe(m_xf, polygonB.vertices[i], m_polygonB.vertices[i]) + Rot.mulToOutUnsafe(m_xf.q, polygonB.normals[i], m_polygonB.normals[i]) + } + + m_radius = 2.0f * Settings.polygonRadius + + manifold.pointCount = 0 + + computeEdgeSeparation(edgeAxis) + + // If no valid normal can be found than this edge should not collide. + if (edgeAxis.type == EPAxis.Type.UNKNOWN) { + return + } + + if (edgeAxis.separation > m_radius) { + return + } + + computePolygonSeparation(polygonAxis) + if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius) { + return + } + + // Use hysteresis for jitter reduction. + val k_relativeTol = 0.98f + val k_absoluteTol = 0.001f + + val primaryAxis: EPAxis + if (polygonAxis.type == EPAxis.Type.UNKNOWN) { + primaryAxis = edgeAxis + } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { + primaryAxis = polygonAxis + } else { + primaryAxis = edgeAxis + } + + val ie0 = ie[0] + val ie1 = ie[1] + + if (primaryAxis.type == EPAxis.Type.EDGE_A) { + manifold.type = Manifold.ManifoldType.FACE_A + + // Search for the polygon normal that is most anti-parallel to the edge normal. + var bestIndex = 0 + var bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]) + for (i in 1 until m_polygonB.count) { + val value = Vec2.dot(m_normal, m_polygonB.normals[i]) + if (value < bestValue) { + bestValue = value + bestIndex = i + } + } + + val i1 = bestIndex + val i2 = if (i1 + 1 < m_polygonB.count) i1 + 1 else 0 + + ie0.v.set(m_polygonB.vertices[i1]) + ie0.id.indexA = 0 + ie0.id.indexB = i1.toByte() + ie0.id.typeA = ContactID.Type.FACE.ordinal.toByte() + ie0.id.typeB = ContactID.Type.VERTEX.ordinal.toByte() + + ie1.v.set(m_polygonB.vertices[i2]) + ie1.id.indexA = 0 + ie1.id.indexB = i2.toByte() + ie1.id.typeA = ContactID.Type.FACE.ordinal.toByte() + ie1.id.typeB = ContactID.Type.VERTEX.ordinal.toByte() + + if (m_front) { + rf.i1 = 0 + rf.i2 = 1 + rf.v1.set(m_v1) + rf.v2.set(m_v2) + rf.normal.set(m_normal1) + } else { + rf.i1 = 1 + rf.i2 = 0 + rf.v1.set(m_v2) + rf.v2.set(m_v1) + rf.normal.set(m_normal1).negateLocal() + } + } else { + manifold.type = Manifold.ManifoldType.FACE_B + + ie0.v.set(m_v1) + ie0.id.indexA = 0 + ie0.id.indexB = primaryAxis.index.toByte() + ie0.id.typeA = ContactID.Type.VERTEX.ordinal.toByte() + ie0.id.typeB = ContactID.Type.FACE.ordinal.toByte() + + ie1.v.set(m_v2) + ie1.id.indexA = 0 + ie1.id.indexB = primaryAxis.index.toByte() + ie1.id.typeA = ContactID.Type.VERTEX.ordinal.toByte() + ie1.id.typeB = ContactID.Type.FACE.ordinal.toByte() + + rf.i1 = primaryAxis.index + rf.i2 = if (rf.i1 + 1 < m_polygonB.count) rf.i1 + 1 else 0 + rf.v1.set(m_polygonB.vertices[rf.i1]) + rf.v2.set(m_polygonB.vertices[rf.i2]) + rf.normal.set(m_polygonB.normals[rf.i1]) + } + + rf.sideNormal1.set(rf.normal.y, -rf.normal.x) + rf.sideNormal2.set(rf.sideNormal1).negateLocal() + rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1) + rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2) + + // Clip incident edge against extruded edge1 side edges. + var np: Int + + // Clip to box side 1 + np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1) + + if (np < Settings.maxManifoldPoints) { + return + } + + // Clip to negative box side 1 + np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2) + + if (np < Settings.maxManifoldPoints) { + return + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == EPAxis.Type.EDGE_A) { + manifold.localNormal.set(rf.normal) + manifold.localPoint.set(rf.v1) + } else { + manifold.localNormal.set(polygonB.normals[rf.i1]) + manifold.localPoint.set(polygonB.vertices[rf.i1]) + } + + var pointCount = 0 + for (i in 0 until Settings.maxManifoldPoints) { + val separation: Float + + separation = Vec2.dot(rf.normal, temp.set(clipPoints2[i].v).subLocal(rf.v1)) + + if (separation <= m_radius) { + val cp = manifold.points[pointCount] + + if (primaryAxis.type == EPAxis.Type.EDGE_A) { + // cp.localPoint = MulT(m_xf, clipPoints2[i].v); + Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint) + cp.id.set(clipPoints2[i].id) + } else { + cp.localPoint.set(clipPoints2[i].v) + cp.id.typeA = clipPoints2[i].id.typeB + cp.id.typeB = clipPoints2[i].id.typeA + cp.id.indexA = clipPoints2[i].id.indexB + cp.id.indexB = clipPoints2[i].id.indexA + } + + ++pointCount + } + } + + manifold.pointCount = pointCount + } + + + fun computeEdgeSeparation(axis: EPAxis) { + axis.type = EPAxis.Type.EDGE_A + axis.index = if (m_front) 0 else 1 + axis.separation = Float.MAX_VALUE + val nx = m_normal.x + val ny = m_normal.y + + for (i in 0 until m_polygonB.count) { + val v = m_polygonB.vertices[i] + val tempx = v.x - m_v1.x + val tempy = v.y - m_v1.y + val s = nx * tempx + ny * tempy + if (s < axis.separation) { + axis.separation = s + } + } + } + + fun computePolygonSeparation(axis: EPAxis) { + axis.type = EPAxis.Type.UNKNOWN + axis.index = -1 + axis.separation = -Float.MAX_VALUE + + perp.x = -m_normal.y + perp.y = m_normal.x + + for (i in 0 until m_polygonB.count) { + val normalB = m_polygonB.normals[i] + val vB = m_polygonB.vertices[i] + n.x = -normalB.x + n.y = -normalB.y + + // float s1 = Vec2.dot(n, temp.set(vB).subLocal(m_v1)); + // float s2 = Vec2.dot(n, temp.set(vB).subLocal(m_v2)); + var tempx = vB.x - m_v1.x + var tempy = vB.y - m_v1.y + val s1 = n.x * tempx + n.y * tempy + tempx = vB.x - m_v2.x + tempy = vB.y - m_v2.y + val s2 = n.x * tempx + n.y * tempy + val s = MathUtils.min(s1, s2) + + if (s > m_radius) { + // No collision + axis.type = EPAxis.Type.EDGE_B + axis.index = i + axis.separation = s + return + } + + // Adjacency + if (n.x * perp.x + n.y * perp.y >= 0.0f) { + if (Vec2.dot(temp.set(n).subLocal(m_upperLimit), m_normal) < -Settings.angularSlop) { + continue + } + } else { + if (Vec2.dot(temp.set(n).subLocal(m_lowerLimit), m_normal) < -Settings.angularSlop) { + continue + } + } + + if (s > axis.separation) { + axis.type = EPAxis.Type.EDGE_B + axis.index = i + axis.separation = s + } + } + } + } + + // #### COLLISION STUFF (not from collision.h or collision.cpp) #### + + // djm pooling + private val d = Vec2() + + companion object { + + val NULL_FEATURE = Int.MAX_VALUE + + /** + * Compute the point states given two manifolds. The states pertain to the transition from + * manifold1 to manifold2. So state1 is either persist or remove while state2 is either add or + * persist. + * + * @param state1 + * @param state2 + * @param manifold1 + * @param manifold2 + */ + + fun getPointStates(state1: Array, state2: Array, + manifold1: Manifold, manifold2: Manifold) { + + for (i in 0 until Settings.maxManifoldPoints) { + state1[i] = PointState.NULL_STATE + state2[i] = PointState.NULL_STATE + } + + // Detect persists and removes. + for (i in 0 until manifold1.pointCount) { + val id = manifold1.points[i].id + + state1[i] = PointState.REMOVE_STATE + + for (j in 0 until manifold2.pointCount) { + if (manifold2.points[j].id.isEqual(id)) { + state1[i] = PointState.PERSIST_STATE + break + } + } + } + + // Detect persists and adds + for (i in 0 until manifold2.pointCount) { + val id = manifold2.points[i].id + + state2[i] = PointState.ADD_STATE + + for (j in 0 until manifold1.pointCount) { + if (manifold1.points[j].id.isEqual(id)) { + state2[i] = PointState.PERSIST_STATE + break + } + } + } + } + + /** + * Clipping for contact manifolds. Sutherland-Hodgman clipping. + * + * @param vOut + * @param vIn + * @param normal + * @param offset + * @return + */ + + fun clipSegmentToLine(vOut: Array, vIn: Array, + normal: Vec2, offset: Float, vertexIndexA: Int): Int { + + // Start with no output points + var numOut = 0 + val vIn0 = vIn[0] + val vIn1 = vIn[1] + val vIn0v = vIn0.v + val vIn1v = vIn1.v + + // Calculate the distance of end points to the line + val distance0 = Vec2.dot(normal, vIn0v) - offset + val distance1 = Vec2.dot(normal, vIn1v) - offset + + // If the points are behind the plane + if (distance0 <= 0.0f) { + vOut[numOut++].set(vIn0) + } + if (distance1 <= 0.0f) { + vOut[numOut++].set(vIn1) + } + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) { + // Find intersection point of edge and plane + val interp = distance0 / (distance0 - distance1) + + val vOutNO = vOut[numOut] + // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + vOutNO.v.x = vIn0v.x + interp * (vIn1v.x - vIn0v.x) + vOutNO.v.y = vIn0v.y + interp * (vIn1v.y - vIn0v.y) + + // VertexA is hitting edgeB. + vOutNO.id.indexA = vertexIndexA.toByte() + vOutNO.id.indexB = vIn0.id.indexB + vOutNO.id.typeA = ContactID.Type.VERTEX.ordinal.toByte() + vOutNO.id.typeB = ContactID.Type.FACE.ordinal.toByte() + ++numOut + } + + return numOut + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ContactID.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ContactID.kt new file mode 100644 index 0000000..8d446c1 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ContactID.kt @@ -0,0 +1,108 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ +package org.jbox2d.collision + +/** + * Contact ids to facilitate warm starting. Note: the ContactFeatures class is just embedded in here + */ +class ContactID : Comparable { + + + var indexA: Byte = 0 + + var indexB: Byte = 0 + + var typeA: Byte = 0 + + var typeB: Byte = 0 + + val key: Int + get() = indexA.toInt() shl 24 or (indexB.toInt() shl 16) or (typeA.toInt() shl 8) or typeB.toInt() + + enum class Type { + VERTEX, FACE + } + + fun isEqual(cid: ContactID): Boolean { + return key == cid.key + } + + constructor() {} + + constructor(c: ContactID) { + set(c) + } + + fun set(c: ContactID) { + indexA = c.indexA + indexB = c.indexB + typeA = c.typeA + typeB = c.typeB + } + + fun flip() { + var tempA = indexA + indexA = indexB + indexB = tempA + tempA = typeA + typeA = typeB + typeB = tempA + } + + /** + * zeros out the data + */ + fun zero() { + indexA = 0 + indexB = 0 + typeA = 0 + typeB = 0 + } + + override fun compareTo(o: ContactID): Int { + return key - o.key + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Distance.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Distance.kt new file mode 100644 index 0000000..ec8dae5 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Distance.kt @@ -0,0 +1,770 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.collision.shapes.* +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.* + +// updated to rev 100 +/** + * This is non-static for faster pooling. To get an instance, use the [SingletonPool], don't + * construct a distance object. + * + * @author Daniel Murphy + */ +class Distance(val stats: Stats = Stats()) { + + private val simplex = Simplex() + private val saveA = IntArray(3) + private val saveB = IntArray(3) + private val closestPoint = Vec2() + private val d = Vec2() + private val temp = Vec2() + private val normal = Vec2() + + /** + * GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. + */ + private inner class SimplexVertex { + val wA = Vec2() // support point in shapeA + val wB = Vec2() // support point in shapeB + val w = Vec2() // wB - wA + var a: Float = 0.toFloat() // barycentric coordinate for closest point + var indexA: Int = 0 // wA index + var indexB: Int = 0 // wB index + + fun set(sv: SimplexVertex) { + wA.set(sv.wA) + wB.set(sv.wB) + w.set(sv.w) + a = sv.a + indexA = sv.indexA + indexB = sv.indexB + } + } + + /** + * Used to warm start Distance. Set count to zero on first call. + * + * @author daniel + */ + class SimplexCache { + /** length or area */ + + var metric: Float = 0.toFloat() + + var count: Int = 0 + /** vertices on shape A */ + + val indexA = IntArray(3) + /** vertices on shape B */ + + val indexB = IntArray(3) + + init { + metric = 0f + count = 0 + indexA[0] = Int.MAX_VALUE + indexA[1] = Int.MAX_VALUE + indexA[2] = Int.MAX_VALUE + indexB[0] = Int.MAX_VALUE + indexB[1] = Int.MAX_VALUE + indexB[2] = Int.MAX_VALUE + } + + fun set(sc: SimplexCache) { + arraycopy(sc.indexA, 0, indexA, 0, indexA.size) + arraycopy(sc.indexB, 0, indexB, 0, indexB.size) + metric = sc.metric + count = sc.count + } + } + + private inner class Simplex { + val m_v1 = SimplexVertex() + val m_v2 = SimplexVertex() + val m_v3 = SimplexVertex() + val vertices = arrayOf(m_v1, m_v2, m_v3) + var m_count: Int = 0 + + private val e12 = Vec2() + + // djm pooled + private val case2 = Vec2() + private val case22 = Vec2() + + // djm pooled, and from above + private val case3 = Vec2() + private val case33 = Vec2() + + // djm pooled, from above + // return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); + val metric: Float + get() { + when (m_count) { + 0 -> { + assert(false) + return 0.0f + } + + 1 -> return 0.0f + + 2 -> return MathUtils.distance(m_v1.w, m_v2.w) + + 3 -> { + case3.set(m_v2.w).subLocal(m_v1.w) + case33.set(m_v3.w).subLocal(m_v1.w) + return Vec2.cross(case3, case33) + } + + else -> { + assert(false) + return 0.0f + } + } + } + + // djm pooled, and from above + private val e13 = Vec2() + private val e23 = Vec2() + private val w1 = Vec2() + private val w2 = Vec2() + private val w3 = Vec2() + + fun readCache(cache: SimplexCache, proxyA: DistanceProxy, transformA: Transform, + proxyB: DistanceProxy, transformB: Transform) { + assert(cache.count <= 3) + + // Copy data from cache. + m_count = cache.count + + for (i in 0 until m_count) { + val v = vertices[i] + v.indexA = cache.indexA[i] + v.indexB = cache.indexB[i] + val wALocal = proxyA.getVertex(v.indexA) + val wBLocal = proxyB.getVertex(v.indexB) + Transform.mulToOutUnsafe(transformA, wALocal, v.wA) + Transform.mulToOutUnsafe(transformB, wBLocal, v.wB) + v.w.set(v.wB).subLocal(v.wA) + v.a = 0.0f + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (m_count > 1) { + val metric1 = cache.metric + val metric2 = metric + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { + // Reset the simplex. + m_count = 0 + } + } + + // If the cache is empty or invalid ... + if (m_count == 0) { + val v = vertices[0] + v.indexA = 0 + v.indexB = 0 + val wALocal = proxyA.getVertex(0) + val wBLocal = proxyB.getVertex(0) + Transform.mulToOutUnsafe(transformA, wALocal, v.wA) + Transform.mulToOutUnsafe(transformB, wBLocal, v.wB) + v.w.set(v.wB).subLocal(v.wA) + m_count = 1 + } + } + + fun writeCache(cache: SimplexCache) { + cache.metric = metric + cache.count = m_count + + for (i in 0 until m_count) { + cache.indexA[i] = vertices[i].indexA + cache.indexB[i] = vertices[i].indexB + } + } + + fun getSearchDirection(out: Vec2) { + when (m_count) { + 1 -> { + out.set(m_v1.w).negateLocal() + return + } + 2 -> { + e12.set(m_v2.w).subLocal(m_v1.w) + // use out for a temp variable real quick + out.set(m_v1.w).negateLocal() + val sgn = Vec2.cross(e12, out) + + if (sgn > 0f) { + // Origin is left of e12. + Vec2.crossToOutUnsafe(1f, e12, out) + return + } else { + // Origin is right of e12. + Vec2.crossToOutUnsafe(e12, 1f, out) + return + } + } + else -> { + assert(false) + out.setZero() + return + } + } + } + + /** + * this returns pooled objects. don't keep or modify them + * + * @return + */ + fun getClosestPoint(out: Vec2) { + when (m_count) { + 0 -> { + assert(false) + out.setZero() + return + } + 1 -> { + out.set(m_v1.w) + return + } + 2 -> { + case22.set(m_v2.w).mulLocal(m_v2.a) + case2.set(m_v1.w).mulLocal(m_v1.a).addLocal(case22) + out.set(case2) + return + } + 3 -> { + out.setZero() + return + } + else -> { + assert(false) + out.setZero() + return + } + } + } + + fun getWitnessPoints(pA: Vec2, pB: Vec2) { + when (m_count) { + 0 -> assert(false) + + 1 -> { + pA.set(m_v1.wA) + pB.set(m_v1.wB) + } + + 2 -> { + case2.set(m_v1.wA).mulLocal(m_v1.a) + pA.set(m_v2.wA).mulLocal(m_v2.a).addLocal(case2) + // m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; + // *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; + case2.set(m_v1.wB).mulLocal(m_v1.a) + pB.set(m_v2.wB).mulLocal(m_v2.a).addLocal(case2) + } + + 3 -> { + pA.set(m_v1.wA).mulLocal(m_v1.a) + case3.set(m_v2.wA).mulLocal(m_v2.a) + case33.set(m_v3.wA).mulLocal(m_v3.a) + pA.addLocal(case3).addLocal(case33) + pB.set(pA) + } + + else -> assert(false) + }// *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; + // *pB = *pA; + } + + // djm pooled from above + /** + * Solve a line segment using barycentric coordinates. + */ + fun solve2() { + // Solve a line segment using barycentric coordinates. + // + // p = a1 * w1 + a2 * w2 + // a1 + a2 = 1 + // + // The vector from the origin to the closest point on the line is + // perpendicular to the line. + // e12 = w2 - w1 + // dot(p, e) = 0 + // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 + // + // 2-by-2 linear system + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // + // Define + // d12_1 = dot(w2, e12) + // d12_2 = -dot(w1, e12) + // d12 = d12_1 + d12_2 + // + // Solution + // a1 = d12_1 / d12 + // a2 = d12_2 / d12 + val w1 = m_v1.w + val w2 = m_v2.w + e12.set(w2).subLocal(w1) + + // w1 region + val d12_2 = -Vec2.dot(w1, e12) + if (d12_2 <= 0.0f) { + // a2 <= 0, so we clamp it to 0 + m_v1.a = 1.0f + m_count = 1 + return + } + + // w2 region + val d12_1 = Vec2.dot(w2, e12) + if (d12_1 <= 0.0f) { + // a1 <= 0, so we clamp it to 0 + m_v2.a = 1.0f + m_count = 1 + m_v1.set(m_v2) + return + } + + // Must be in e12 region. + val inv_d12 = 1.0f / (d12_1 + d12_2) + m_v1.a = d12_1 * inv_d12 + m_v2.a = d12_2 * inv_d12 + m_count = 2 + } + + /** + * Solve a line segment using barycentric coordinates.

+ * Possible regions:

+ * - points[2]

+ * - edge points[0]-points[2]

+ * - edge points[1]-points[2]

+ * - inside the triangle + */ + fun solve3() { + w1.set(m_v1.w) + w2.set(m_v2.w) + w3.set(m_v3.w) + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + e12.set(w2).subLocal(w1) + val w1e12 = Vec2.dot(w1, e12) + val w2e12 = Vec2.dot(w2, e12) + val d12_1 = w2e12 + val d12_2 = -w1e12 + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + e13.set(w3).subLocal(w1) + val w1e13 = Vec2.dot(w1, e13) + val w3e13 = Vec2.dot(w3, e13) + val d13_1 = w3e13 + val d13_2 = -w1e13 + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + e23.set(w3).subLocal(w2) + val w2e23 = Vec2.dot(w2, e23) + val w3e23 = Vec2.dot(w3, e23) + val d23_1 = w3e23 + val d23_2 = -w2e23 + + // Triangle123 + val n123 = Vec2.cross(e12, e13) + + val d123_1 = n123 * Vec2.cross(w2, w3) + val d123_2 = n123 * Vec2.cross(w3, w1) + val d123_3 = n123 * Vec2.cross(w1, w2) + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) { + m_v1.a = 1.0f + m_count = 1 + return + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { + val inv_d12 = 1.0f / (d12_1 + d12_2) + m_v1.a = d12_1 * inv_d12 + m_v2.a = d12_2 * inv_d12 + m_count = 2 + return + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { + val inv_d13 = 1.0f / (d13_1 + d13_2) + m_v1.a = d13_1 * inv_d13 + m_v3.a = d13_2 * inv_d13 + m_count = 2 + m_v2.set(m_v3) + return + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) { + m_v2.a = 1.0f + m_count = 1 + m_v1.set(m_v2) + return + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) { + m_v3.a = 1.0f + m_count = 1 + m_v1.set(m_v3) + return + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { + val inv_d23 = 1.0f / (d23_1 + d23_2) + m_v2.a = d23_1 * inv_d23 + m_v3.a = d23_2 * inv_d23 + m_count = 2 + m_v1.set(m_v3) + return + } + + // Must be in triangle123 + val inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3) + m_v1.a = d123_1 * inv_d123 + m_v2.a = d123_2 * inv_d123 + m_v3.a = d123_3 * inv_d123 + m_count = 3 + } + } + + /** + * A distance proxy is used by the GJK algorithm. It encapsulates any shape. TODO: see if we can + * just do assignments with m_vertices, instead of copying stuff over + * + * @author daniel + */ + class DistanceProxy { + + val m_vertices: Array = Array(Settings.maxPolygonVertices) { Vec2() } + /** + * Get the vertex count. + * + * @return + */ + + var vertexCount: Int = 0 + + var m_radius: Float = 0f + + val m_buffer: Array = Array(2) { Vec2() } + + /** + * Initialize the proxy using the given shape. The shape must remain in scope while the proxy is + * in use. + */ + fun set(shape: Shape, index: Int) { + when (shape.getType()) { + ShapeType.CIRCLE -> { + val circle = shape as CircleShape + m_vertices[0].set(circle.p) + vertexCount = 1 + m_radius = circle.radius + } + ShapeType.POLYGON -> { + val poly = shape as PolygonShape + vertexCount = poly.count + m_radius = poly.radius + for (i in 0 until vertexCount) { + m_vertices[i].set(poly.vertices[i]) + } + } + ShapeType.CHAIN -> { + val chain = shape as ChainShape + assert(0 <= index && index < chain.count) + + m_buffer[0] = chain.vertices!![index] + if (index + 1 < chain.count) { + m_buffer[1] = chain.vertices!![index + 1] + } else { + m_buffer[1] = chain.vertices!![0] + } + + m_vertices[0].set(m_buffer[0]) + m_vertices[1].set(m_buffer[1]) + vertexCount = 2 + m_radius = chain.radius + } + ShapeType.EDGE -> { + val edge = shape as EdgeShape + m_vertices[0].set(edge.vertex1) + m_vertices[1].set(edge.vertex2) + vertexCount = 2 + m_radius = edge.radius + } + else -> assert(false) + } + } + + /** + * Get the supporting vertex index in the given direction. + * + * @param d + * @return + */ + fun getSupport(d: Vec2): Int { + var bestIndex = 0 + var bestValue = Vec2.dot(m_vertices[0], d) + for (i in 1 until vertexCount) { + val value = Vec2.dot(m_vertices[i], d) + if (value > bestValue) { + bestIndex = i + bestValue = value + } + } + + return bestIndex + } + + /** + * Get the supporting vertex in the given direction. + * + * @param d + * @return + */ + fun getSupportVertex(d: Vec2): Vec2 { + var bestIndex = 0 + var bestValue = Vec2.dot(m_vertices[0], d) + for (i in 1 until vertexCount) { + val value = Vec2.dot(m_vertices[i], d) + if (value > bestValue) { + bestIndex = i + bestValue = value + } + } + + return m_vertices[bestIndex] + } + + /** + * Get a vertex by index. Used by Distance. + * + * @param index + * @return + */ + fun getVertex(index: Int): Vec2 { + assert(0 <= index && index < vertexCount) + return m_vertices[index] + } + } + + /** + * Compute the closest points between two shapes. Supports any combination of: CircleShape and + * PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to + * zero. + * + * @param output + * @param cache + * @param input + */ + fun distance(output: DistanceOutput, cache: SimplexCache, + input: DistanceInput) { + stats.GJK_CALLS++ + + val proxyA = input.proxyA + val proxyB = input.proxyB + + val transformA = input.transformA + val transformB = input.transformB + + // Initialize the simplex. + simplex.readCache(cache, proxyA, transformA, proxyB, transformB) + + // Get simplex vertices as an array. + val vertices = simplex.vertices + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + // (pooled above) + var saveCount = 0 + + simplex.getClosestPoint(closestPoint) + var distanceSqr1 = closestPoint.lengthSquared() + var distanceSqr2 = distanceSqr1 + + // Main iteration loop + var iter = 0 + while (iter < MAX_ITERS) { + + // Copy simplex so we can identify duplicates. + saveCount = simplex.m_count + for (i in 0 until saveCount) { + saveA[i] = vertices[i].indexA + saveB[i] = vertices[i].indexB + } + + when (simplex.m_count) { + 1 -> { + } + 2 -> simplex.solve2() + 3 -> simplex.solve3() + else -> assert(false) + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.m_count == 3) { + break + } + + // Compute closest point. + simplex.getClosestPoint(closestPoint) + distanceSqr2 = closestPoint.lengthSquared() + + // ensure progress + if (distanceSqr2 >= distanceSqr1) { + // break; + } + distanceSqr1 = distanceSqr2 + + // get search direction; + simplex.getSearchDirection(d) + + // Ensure the search direction is numerically fit. + if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break + } + /* + * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = + * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, + * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = + * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, + * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; + */ + + // Compute a tentative new simplex vertex using support points. + val vertex = vertices[simplex.m_count] + + Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp) + vertex.indexA = proxyA.getSupport(temp) + Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA) + // Vec2 wBLocal; + Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp) + vertex.indexB = proxyB.getSupport(temp) + Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB) + vertex.w.set(vertex.wB).subLocal(vertex.wA) + + // Iteration count is equated to the number of support point calls. + ++iter + ++stats.GJK_ITERS + + // Check for duplicate support points. This is the main termination criteria. + var duplicate = false + for (i in 0 until saveCount) { + if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) { + duplicate = true + break + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) { + break + } + + // New vertex is ok and needed. + ++simplex.m_count + } + + stats.GJK_MAX_ITERS = MathUtils.max(stats.GJK_MAX_ITERS, iter) + + // Prepare output. + simplex.getWitnessPoints(output.pointA, output.pointB) + output.distance = MathUtils.distance(output.pointA, output.pointB) + output.iterations = iter + + // Cache the simplex. + simplex.writeCache(cache) + + // Apply radii if requested. + if (input.useRadii) { + val rA = proxyA.m_radius + val rB = proxyB.m_radius + + if (output.distance > rA + rB && output.distance > Settings.EPSILON) { + // Shapes are still no overlapped. + // Move the witness points to the outer surface. + output.distance -= rA + rB + normal.set(output.pointB).subLocal(output.pointA) + normal.normalize() + temp.set(normal).mulLocal(rA) + output.pointA.addLocal(temp) + temp.set(normal).mulLocal(rB) + output.pointB.subLocal(temp) + } else { + // Shapes are overlapped when radii are considered. + // Move the witness points to the middle. + // Vec2 p = 0.5f * (output.pointA + output.pointB); + output.pointA.addLocal(output.pointB).mulLocal(.5f) + output.pointB.set(output.pointA) + output.distance = 0.0f + } + } + } + + companion object { + + val MAX_ITERS = 20 + + } + + class Stats { + var GJK_CALLS = 0 + + var GJK_ITERS = 0 + + var GJK_MAX_ITERS = 20 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceInput.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceInput.kt new file mode 100644 index 0000000..29a8978 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceInput.kt @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.collision.Distance.DistanceProxy +import org.jbox2d.common.Transform + +/** + * Input for Distance. + * You have to option to use the shape radii + * in the computation. + * + */ +class DistanceInput { + + var proxyA = DistanceProxy() + + var proxyB = DistanceProxy() + + var transformA = Transform() + + var transformB = Transform() + + var useRadii: Boolean = false +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceOutput.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceOutput.kt new file mode 100644 index 0000000..616cf59 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/DistanceOutput.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.Vec2 + +/** + * Output for Distance. + * @author Daniel + */ +class DistanceOutput { + /** Closest point on shapeA */ + + val pointA = Vec2() + + /** Closest point on shapeB */ + + val pointB = Vec2() + + + var distance: Float = 0.toFloat() + + /** number of gjk iterations used */ + + var iterations: Int = 0 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Manifold.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Manifold.kt new file mode 100644 index 0000000..2299e96 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/Manifold.kt @@ -0,0 +1,115 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 + +/** + * A manifold for two touching convex shapes. Box2D supports multiple types of contact: + * + * * clip point versus plane with radius + * * point versus point with radius (circles) + * + * The local point usage depends on the manifold type: + * + * * e_circles: the local center of circleA + * * e_faceA: the center of faceA + * * e_faceB: the center of faceB + * + * Similarly the local normal usage: + * + * * e_circles: not used + * * e_faceA: the normal on polygonA + * * e_faceB: the normal on polygonB + * + * We store contacts in this way so that position correction can account for movement, which is + * critical for continuous physics. All contact scenarios must be expressed in one of these types. + * This structure is stored across time steps, so we keep it small. + */ +class Manifold { + + /** The points of contact. */ + + val points: Array + + /** not use for Type::e_points */ + + val localNormal: Vec2 + + /** usage depends on manifold type */ + + val localPoint: Vec2 + + + var type: ManifoldType = ManifoldType.CIRCLES + + /** The number of manifold points. */ + + var pointCount: Int = 0 + + enum class ManifoldType { + CIRCLES, FACE_A, FACE_B + } + + /** + * creates a manifold with 0 points, with it's points array full of instantiated ManifoldPoints. + */ + constructor() { + points = Array(Settings.maxManifoldPoints) { ManifoldPoint() } + localNormal = Vec2() + localPoint = Vec2() + pointCount = 0 + } + + /** + * Creates this manifold as a copy of the other + * + * @param other + */ + constructor(other: Manifold) { + localNormal = other.localNormal.clone() + localPoint = other.localPoint.clone() + pointCount = other.pointCount + type = other.type + // djm: this is correct now + points = Array(Settings.maxManifoldPoints) { ManifoldPoint(other.points[it]) } + } + + /** + * copies this manifold from the given one + * + * @param cp manifold to copy from + */ + fun set(cp: Manifold) { + for (i in 0 until cp.pointCount) { + points[i].set(cp.points[i]) + } + + type = cp.type + localNormal.set(cp.localNormal) + localPoint.set(cp.localPoint) + pointCount = cp.pointCount + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ManifoldPoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ManifoldPoint.kt new file mode 100644 index 0000000..3c913b5 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/ManifoldPoint.kt @@ -0,0 +1,109 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.collision + +import org.jbox2d.common.Vec2 + +// updated to rev 100 +/** + * A manifold point is a contact point belonging to a contact + * manifold. It holds details related to the geometry and dynamics + * of the contact points. + * The local point usage depends on the manifold type: + * * e_circles: the local center of circleB + * * e_faceA: the local center of cirlceB or the clip point of polygonB + * * e_faceB: the clip point of polygonA + * This structure is stored across time steps, so we keep it small.

+ * Note: the impulses are used for internal caching and may not + * provide reliable contact forces, especially for high speed collisions. + */ +class ManifoldPoint { + /** usage depends on manifold type */ + + val localPoint: Vec2 + /** the non-penetration impulse */ + + var normalImpulse: Float = 0.toFloat() + /** the friction impulse */ + + var tangentImpulse: Float = 0.toFloat() + /** uniquely identifies a contact point between two shapes */ + + val id: ContactID + + /** + * Blank manifold point with everything zeroed out. + */ + constructor() { + localPoint = Vec2() + tangentImpulse = 0f + normalImpulse = tangentImpulse + id = ContactID() + } + + /** + * Creates a manifold point as a copy of the given point + * @param cp point to copy from + */ + constructor(cp: ManifoldPoint) { + localPoint = cp.localPoint.clone() + normalImpulse = cp.normalImpulse + tangentImpulse = cp.tangentImpulse + id = ContactID(cp.id) + } + + /** + * Sets this manifold point form the given one + * @param cp the point to copy from + */ + fun set(cp: ManifoldPoint) { + localPoint.set(cp.localPoint) + normalImpulse = cp.normalImpulse + tangentImpulse = cp.tangentImpulse + id.set(cp.id) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastInput.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastInput.kt new file mode 100644 index 0000000..4f4dede --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastInput.kt @@ -0,0 +1,51 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.Vec2 + +// updated to rev 100 +/** + * Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + */ +class RayCastInput { + + val p1: Vec2 + + val p2: Vec2 + + var maxFraction: Float = 0.toFloat() + + init { + p1 = Vec2() + p2 = Vec2() + maxFraction = 0f + } + + fun set(rci: RayCastInput) { + p1.set(rci.p1) + p2.set(rci.p2) + maxFraction = rci.maxFraction + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastOutput.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastOutput.kt new file mode 100644 index 0000000..98e64d8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/RayCastOutput.kt @@ -0,0 +1,48 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.Vec2 + +// updated to rev 100 +/** + * Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 + * come from b2RayCastInput. + */ +class RayCastOutput { + + val normal: Vec2 + + var fraction: Float = 0.toFloat() + + init { + normal = Vec2() + fraction = 0f + } + + fun set(rco: RayCastOutput) { + normal.set(rco.normal) + fraction = rco.fraction + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/TimeOfImpact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/TimeOfImpact.kt new file mode 100644 index 0000000..6adb566 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/TimeOfImpact.kt @@ -0,0 +1,535 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.collision.Distance.DistanceProxy +import org.jbox2d.collision.Distance.SimplexCache +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Sweep +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.* +import org.jbox2d.pooling.IWorldPool + +/** + * Class used for computing the time of impact. This class should not be constructed usually, just + * retrieve from the [IWorldPool.getTimeOfImpact]. + * + * @author daniel + */ +class TimeOfImpact(private val pool: IWorldPool, private val stats: TimeOfImpact.Stats = TimeOfImpact.Stats()) { + + + // djm pooling + private val cache = SimplexCache() + private val distanceInput = DistanceInput() + private val xfA = Transform() + private val xfB = Transform() + private val distanceOutput = DistanceOutput() + private val fcn = SeparationFunction() + private val indexes = IntArray(2) + private val sweepA = Sweep() + private val sweepB = Sweep() + + /** + * Input parameters for TOI + * + * @author Daniel Murphy + */ + class TOIInput { + + val proxyA = DistanceProxy() + + val proxyB = DistanceProxy() + + val sweepA = Sweep() + + val sweepB = Sweep() + /** + * defines sweep interval [0, tMax] + */ + + var tMax: Float = 0.toFloat() + } + + enum class TOIOutputState { + UNKNOWN, FAILED, OVERLAPPED, TOUCHING, SEPARATED + } + + /** + * Output parameters for TimeOfImpact + * + * @author daniel + */ + class TOIOutput { + + var state: TOIOutputState? = null + + var t: Float = 0.toFloat() + } + + /** + * Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction + * between [0,tMax]. This uses a swept separating axis and may miss some intermediate, + * non-tunneling collision. If you change the time interval, you should call this function again. + * Note: use Distance to compute the contact point and normal at the time of impact. + * + * @param output + * @param input + */ + fun timeOfImpact(output: TOIOutput, input: TOIInput) { + // CCD via the local separating axis method. This seeks progression + // by computing the largest time at which separation is maintained. + + ++stats.toiCalls + + output.state = TOIOutputState.UNKNOWN + output.t = input.tMax + + val proxyA = input.proxyA + val proxyB = input.proxyB + + sweepA.set(input.sweepA) + sweepB.set(input.sweepB) + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.normalize() + sweepB.normalize() + + val tMax = input.tMax + + val totalRadius = proxyA.m_radius + proxyB.m_radius + // djm: whats with all these constants? + val target = MathUtils.max(Settings.linearSlop, totalRadius - 3.0f * Settings.linearSlop) + val tolerance = 0.25f * Settings.linearSlop + + assert(target > tolerance) + + var t1 = 0f + var iter = 0 + + cache.count = 0 + distanceInput.proxyA = input.proxyA + distanceInput.proxyB = input.proxyB + distanceInput.useRadii = false + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + while (true) { + sweepA.getTransform(xfA, t1) + sweepB.getTransform(xfB, t1) + // System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n", + // sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y); + // Get the distance between shapes. We can also use the results + // to get a separating axis + distanceInput.transformA = xfA + distanceInput.transformB = xfB + pool.distance.distance(distanceOutput, cache, distanceInput) + + // System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n", + // distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y, + // distanceOutput.pointB.x, distanceOutput.pointB.y, + // distanceOutput.iterations); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.distance <= 0f) { + // Failure! + output.state = TOIOutputState.OVERLAPPED + output.t = 0f + break + } + + if (distanceOutput.distance < target + tolerance) { + // Victory! + output.state = TOIOutputState.TOUCHING + output.t = t1 + break + } + + // Initialize the separating axis. + fcn.initialize(cache, proxyA, sweepA, proxyB, sweepB, t1) + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of + // vertices. + var done = false + var t2 = tMax + var pushBackIter = 0 + while (true) { + + // Find the deepest point at t2. Store the witness point indices. + var s2 = fcn.findMinSeparation(indexes, t2) + // System.out.printf("s2: %f\n", s2); + // Is the final configuration separated? + if (s2 > target + tolerance) { + // Victory! + output.state = TOIOutputState.SEPARATED + output.t = tMax + done = true + break + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) { + // Advance the sweeps + t1 = t2 + break + } + + // Compute the initial separation of the witness points. + var s1 = fcn.evaluate(indexes[0], indexes[1], t1) + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + // System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target, + // tolerance); + if (s1 < target - tolerance) { + output.state = TOIOutputState.FAILED + output.t = t1 + done = true + break + } + + // Check for touching + if (s1 <= target + tolerance) { + // Victory! t1 should hold the TOI (could be 0.0). + output.state = TOIOutputState.TOUCHING + output.t = t1 + done = true + break + } + + // Compute 1D root of: f(x) - target = 0 + var rootIterCount = 0 + var a1 = t1 + var a2 = t2 + while (true) { + // Use a mix of the secant rule and bisection. + val t: Float + if (rootIterCount and 1 == 1) { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1) + } else { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2) + } + + ++rootIterCount + ++stats.toiRootIters + + val s = fcn.evaluate(indexes[0], indexes[1], t) + + if (MathUtils.abs(s - target) < tolerance) { + // t2 holds a tentative value for t1 + t2 = t + break + } + + // Ensure we continue to bracket the root. + if (s > target) { + a1 = t + s1 = s + } else { + a2 = t + s2 = s + } + + if (rootIterCount == MAX_ROOT_ITERATIONS) { + break + } + } + + stats.toiMaxRootIters = MathUtils.max(stats.toiMaxRootIters, rootIterCount) + + ++pushBackIter + + if (pushBackIter == Settings.maxPolygonVertices || rootIterCount == MAX_ROOT_ITERATIONS) { + break + } + } + + ++iter + ++stats.toiIters + + if (done) { + // System.out.println("done"); + break + } + + if (iter == MAX_ITERATIONS) { + // System.out.println("failed, root finder stuck"); + // Root finder got stuck. Semi-victory. + output.state = TOIOutputState.FAILED + output.t = t1 + break + } + } + + // System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s) + stats.toiMaxIters = MathUtils.max(stats.toiMaxIters, iter) + } + + companion object { + val MAX_ITERATIONS = 20 + val MAX_ROOT_ITERATIONS = 50 + } + + class Stats { + var toiCalls = 0 + var toiIters = 0 + var toiMaxIters = 0 + var toiRootIters = 0 + var toiMaxRootIters = 0 + } +} + + +internal enum class Type { + POINTS, FACE_A, FACE_B +} + + +internal class SeparationFunction { + + lateinit var m_proxyA: DistanceProxy + lateinit var m_proxyB: DistanceProxy + lateinit var m_type: Type + val m_localPoint = Vec2() + val m_axis = Vec2() + lateinit var m_sweepA: Sweep + lateinit var m_sweepB: Sweep + + // djm pooling + private val localPointA = Vec2() + private val localPointB = Vec2() + private val pointA = Vec2() + private val pointB = Vec2() + private val localPointA1 = Vec2() + private val localPointA2 = Vec2() + private val normal = Vec2() + private val localPointB1 = Vec2() + private val localPointB2 = Vec2() + private val temp = Vec2() + private val xfa = Transform() + private val xfb = Transform() + + private val axisA = Vec2() + private val axisB = Vec2() + + // TODO_ERIN might not need to return the separation + + fun initialize(cache: SimplexCache, proxyA: DistanceProxy, sweepA: Sweep, + proxyB: DistanceProxy, sweepB: Sweep, t1: Float): Float { + m_proxyA = proxyA + m_proxyB = proxyB + val count = cache.count + assert(0 < count && count < 3) + + m_sweepA = sweepA + m_sweepB = sweepB + + m_sweepA.getTransform(xfa, t1) + m_sweepB.getTransform(xfb, t1) + + // log.debug("initializing separation.\n" + + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" + // "distance: "+proxyA. + + if (count == 1) { + m_type = Type.POINTS + /* + * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = + * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2 + * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize(); + */ + localPointA.set(m_proxyA.getVertex(cache.indexA[0])) + localPointB.set(m_proxyB.getVertex(cache.indexB[0])) + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + m_axis.set(pointB).subLocal(pointA) + val s = m_axis.normalize() + return s + } else if (cache.indexA[0] == cache.indexA[1]) { + // Two points on B and one on A. + m_type = Type.FACE_B + + localPointB1.set(m_proxyB.getVertex(cache.indexB[0])) + localPointB2.set(m_proxyB.getVertex(cache.indexB[1])) + + temp.set(localPointB2).subLocal(localPointB1) + Vec2.crossToOutUnsafe(temp, 1f, m_axis) + m_axis.normalize() + + Rot.mulToOutUnsafe(xfb.q, m_axis, normal) + + m_localPoint.set(localPointB1).addLocal(localPointB2).mulLocal(.5f) + Transform.mulToOutUnsafe(xfb, m_localPoint, pointB) + + localPointA.set(proxyA.getVertex(cache.indexA[0])) + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + + temp.set(pointA).subLocal(pointB) + var s = Vec2.dot(temp, normal) + if (s < 0.0f) { + m_axis.negateLocal() + s = -s + } + return s + } else { + // Two points on A and one or two points on B. + m_type = Type.FACE_A + + localPointA1.set(m_proxyA.getVertex(cache.indexA[0])) + localPointA2.set(m_proxyA.getVertex(cache.indexA[1])) + + temp.set(localPointA2).subLocal(localPointA1) + Vec2.crossToOutUnsafe(temp, 1.0f, m_axis) + m_axis.normalize() + + Rot.mulToOutUnsafe(xfa.q, m_axis, normal) + + m_localPoint.set(localPointA1).addLocal(localPointA2).mulLocal(.5f) + Transform.mulToOutUnsafe(xfa, m_localPoint, pointA) + + localPointB.set(m_proxyB.getVertex(cache.indexB[0])) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + + temp.set(pointB).subLocal(pointA) + var s = Vec2.dot(temp, normal) + if (s < 0.0f) { + m_axis.negateLocal() + s = -s + } + return s + } + } + + // float FindMinSeparation(int* indexA, int* indexB, float t) const + fun findMinSeparation(indexes: IntArray, t: Float): Float { + + m_sweepA.getTransform(xfa, t) + m_sweepB.getTransform(xfb, t) + + when (m_type) { + Type.POINTS -> { + Rot.mulTransUnsafe(xfa.q, m_axis, axisA) + Rot.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB) + m_axis.negateLocal() + + indexes[0] = m_proxyA.getSupport(axisA) + indexes[1] = m_proxyB.getSupport(axisB) + + localPointA.set(m_proxyA.getVertex(indexes[0])) + localPointB.set(m_proxyB.getVertex(indexes[1])) + + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + + val separation = Vec2.dot(pointB.subLocal(pointA), m_axis) + return separation + } + Type.FACE_A -> { + Rot.mulToOutUnsafe(xfa.q, m_axis, normal) + Transform.mulToOutUnsafe(xfa, m_localPoint, pointA) + + Rot.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB) + normal.negateLocal() + + indexes[0] = -1 + indexes[1] = m_proxyB.getSupport(axisB) + + localPointB.set(m_proxyB.getVertex(indexes[1])) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + + val separation = Vec2.dot(pointB.subLocal(pointA), normal) + return separation + } + Type.FACE_B -> { + Rot.mulToOutUnsafe(xfb.q, m_axis, normal) + Transform.mulToOutUnsafe(xfb, m_localPoint, pointB) + + Rot.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA) + normal.negateLocal() + + indexes[1] = -1 + indexes[0] = m_proxyA.getSupport(axisA) + + localPointA.set(m_proxyA.getVertex(indexes[0])) + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + + val separation = Vec2.dot(pointA.subLocal(pointB), normal) + return separation + } + else -> { + assert(false) + indexes[0] = -1 + indexes[1] = -1 + return 0f + } + } + } + + fun evaluate(indexA: Int, indexB: Int, t: Float): Float { + m_sweepA.getTransform(xfa, t) + m_sweepB.getTransform(xfb, t) + + when (m_type) { + Type.POINTS -> { + localPointA.set(m_proxyA.getVertex(indexA)) + localPointB.set(m_proxyB.getVertex(indexB)) + + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + + val separation = Vec2.dot(pointB.subLocal(pointA), m_axis) + return separation + } + Type.FACE_A -> { + Rot.mulToOutUnsafe(xfa.q, m_axis, normal) + Transform.mulToOutUnsafe(xfa, m_localPoint, pointA) + + localPointB.set(m_proxyB.getVertex(indexB)) + Transform.mulToOutUnsafe(xfb, localPointB, pointB) + val separation = Vec2.dot(pointB.subLocal(pointA), normal) + return separation + } + Type.FACE_B -> { + Rot.mulToOutUnsafe(xfb.q, m_axis, normal) + Transform.mulToOutUnsafe(xfb, m_localPoint, pointB) + + localPointA.set(m_proxyA.getVertex(indexA)) + Transform.mulToOutUnsafe(xfa, localPointA, pointA) + + val separation = Vec2.dot(pointA.subLocal(pointB), normal) + return separation + } + else -> { + assert(false) + return 0f + } + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/WorldManifold.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/WorldManifold.kt new file mode 100644 index 0000000..db6e564 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/WorldManifold.kt @@ -0,0 +1,186 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 + +/** + * This is used to compute the current state of a contact manifold. + * + * @author daniel + */ +class WorldManifold { + /** + * World vector pointing from A to B + */ + + val normal: Vec2 = Vec2() + + /** + * World contact point (point of intersection) + */ + + val points: Array = Array(Settings.maxManifoldPoints) { Vec2() } + + /** + * A negative value indicates overlap, in meters. + */ + + val separations: FloatArray = FloatArray(Settings.maxManifoldPoints) + + private val pool3 = Vec2() + private val pool4 = Vec2() + + fun initialize(manifold: Manifold, xfA: Transform, radiusA: Float, + xfB: Transform, radiusB: Float) { + if (manifold.pointCount == 0) { + return + } + + when (manifold.type) { + Manifold.ManifoldType.CIRCLES -> { + val pointA = pool3 + val pointB = pool4 + + normal.x = 1f + normal.y = 0f + val v = manifold.localPoint + // Transform.mulToOutUnsafe(xfA, manifold.localPoint, pointA); + // Transform.mulToOutUnsafe(xfB, manifold.points[0].localPoint, pointB); + pointA.x = xfA.q.c * v.x - xfA.q.s * v.y + xfA.p.x + pointA.y = xfA.q.s * v.x + xfA.q.c * v.y + xfA.p.y + val mp0p = manifold.points[0].localPoint + pointB.x = xfB.q.c * mp0p.x - xfB.q.s * mp0p.y + xfB.p.x + pointB.y = xfB.q.s * mp0p.x + xfB.q.c * mp0p.y + xfB.p.y + + if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { + normal.x = pointB.x - pointA.x + normal.y = pointB.y - pointA.y + normal.normalize() + } + + val cAx = normal.x * radiusA + pointA.x + val cAy = normal.y * radiusA + pointA.y + + val cBx = -normal.x * radiusB + pointB.x + val cBy = -normal.y * radiusB + pointB.y + + points[0].x = (cAx + cBx) * .5f + points[0].y = (cAy + cBy) * .5f + separations[0] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y + } + Manifold.ManifoldType.FACE_A -> { + val planePoint = pool3 + + Rot.mulToOutUnsafe(xfA.q, manifold.localNormal, normal) + Transform.mulToOut(xfA, manifold.localPoint, planePoint) + + val clipPoint = pool4 + + for (i in 0 until manifold.pointCount) { + // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); + // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, + // normal)) * normal; + // b2Vec2 cB = clipPoint - radiusB * normal; + // points[i] = 0.5f * (cA + cB); + Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint) + // use cA as temporary for now + // cA.set(clipPoint).subLocal(planePoint); + // float scalar = radiusA - Vec2.dot(cA, normal); + // cA.set(normal).mulLocal(scalar).addLocal(clipPoint); + // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal(); + // points[i].set(cA).addLocal(cB).mulLocal(0.5f); + + val scalar = radiusA - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y) + + val cAx = normal.x * scalar + clipPoint.x + val cAy = normal.y * scalar + clipPoint.y + + val cBx = -normal.x * radiusB + clipPoint.x + val cBy = -normal.y * radiusB + clipPoint.y + + points[i].x = (cAx + cBx) * .5f + points[i].y = (cAy + cBy) * .5f + separations[i] = (cBx - cAx) * normal.x + (cBy - cAy) * normal.y + } + } + Manifold.ManifoldType.FACE_B -> { + val planePoint = pool3 + Rot.mulToOutUnsafe(xfB.q, manifold.localNormal, normal) + Transform.mulToOut(xfB, manifold.localPoint, planePoint) + + // final Mat22 R = xfB.q; + // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y; + // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y; + // final Vec2 v = manifold.localPoint; + // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; + // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; + + val clipPoint = pool4 + + for (i in 0 until manifold.pointCount) { + // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); + // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, + // normal)) * normal; + // b2Vec2 cA = clipPoint - radiusA * normal; + // points[i] = 0.5f * (cA + cB); + + Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint) + // cB.set(clipPoint).subLocal(planePoint); + // float scalar = radiusB - Vec2.dot(cB, normal); + // cB.set(normal).mulLocal(scalar).addLocal(clipPoint); + // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal(); + // points[i].set(cA).addLocal(cB).mulLocal(0.5f); + + // points[i] = 0.5f * (cA + cB); + + // + // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x * + // manifold.points[i].localPoint.y; + // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y * + // manifold.points[i].localPoint.y; + + val scalar = radiusB - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y) + + val cBx = normal.x * scalar + clipPoint.x + val cBy = normal.y * scalar + clipPoint.y + + val cAx = -normal.x * radiusA + clipPoint.x + val cAy = -normal.y * radiusA + clipPoint.y + + points[i].x = (cAx + cBx) * .5f + points[i].y = (cAy + cBy) * .5f + separations[i] = (cAx - cBx) * normal.x + (cAy - cBy) * normal.y + } + // Ensure normal points from A to B. + normal.x = -normal.x + normal.y = -normal.y + } + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhase.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhase.kt new file mode 100644 index 0000000..5755cd0 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhase.kt @@ -0,0 +1,96 @@ +package org.jbox2d.collision.broadphase + +import org.jbox2d.callbacks.DebugDraw +import org.jbox2d.callbacks.PairCallback +import org.jbox2d.callbacks.TreeCallback +import org.jbox2d.callbacks.TreeRayCastCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.common.Vec2 + + +interface BroadPhase { + + /** + * Get the number of proxies. + * + * @return + */ + val proxyCount: Int + + /** + * Get the height of the embedded tree. + * + * @return + */ + val treeHeight: Int + + val treeBalance: Int + + val treeQuality: Float + + /** + * Create a proxy with an initial AABB. Pairs are not reported until updatePairs is called. + * + * @param aabb + * @param userData + * @return + */ + fun createProxy(aabb: AABB, userData: Any): Int + + /** + * Destroy a proxy. It is up to the client to remove any pairs. + * + * @param proxyId + */ + fun destroyProxy(proxyId: Int) + + /** + * Call MoveProxy as many times as you like, then when you are done call UpdatePairs to finalized + * the proxy pairs (for your time step). + */ + fun moveProxy(proxyId: Int, aabb: AABB, displacement: Vec2) + + fun touchProxy(proxyId: Int) + + fun getUserData(proxyId: Int): Any? + + fun getFatAABB(proxyId: Int): AABB + + fun testOverlap(proxyIdA: Int, proxyIdB: Int): Boolean + + fun drawTree(argDraw: DebugDraw) + + /** + * Update the pairs. This results in pair callbacks. This can only add pairs. + * + * @param callback + */ + fun updatePairs(callback: PairCallback) + + /** + * Query an AABB for overlapping proxies. The callback class is called for each proxy that + * overlaps the supplied AABB. + * + * @param callback + * @param aabb + */ + fun query(callback: TreeCallback, aabb: AABB) + + /** + * Ray-cast against the proxies in the tree. This relies on the callback to perform a exact + * ray-cast in the case were the proxy contains a shape. The callback also performs the any + * collision filtering. This has performance roughly equal to k * log(n), where k is the number of + * collisions and n is the number of proxies in the tree. + * + * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + * @param callback a callback class that is called for each proxy that is hit by the ray. + */ + fun raycast(callback: TreeRayCastCallback, input: RayCastInput) + + companion object { + + + val NULL_PROXY = -1 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhaseStrategy.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhaseStrategy.kt new file mode 100644 index 0000000..1b7dcbf --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/BroadPhaseStrategy.kt @@ -0,0 +1,88 @@ +package org.jbox2d.collision.broadphase + +import org.jbox2d.callbacks.DebugDraw +import org.jbox2d.callbacks.TreeCallback +import org.jbox2d.callbacks.TreeRayCastCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.common.Vec2 + +interface BroadPhaseStrategy { + + /** + * Compute the height of the binary tree in O(N) time. Should not be called often. + * + * @return + */ + val height: Int + + /** + * Get the maximum balance of an node in the tree. The balance is the difference in height of the + * two children of a node. + * + * @return + */ + val maxBalance: Int + + /** + * Get the ratio of the sum of the node areas to the root area. + * + * @return + */ + val areaRatio: Float + + /** + * Create a proxy. Provide a tight fitting AABB and a userData pointer. + * + * @param aabb + * @param userData + * @return + */ + fun createProxy(aabb: AABB, userData: Any): Int + + /** + * Destroy a proxy + * + * @param proxyId + */ + fun destroyProxy(proxyId: Int) + + /** + * Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, then the + * proxy is removed from the tree and re-inserted. Otherwise the function returns immediately. + * + * @return true if the proxy was re-inserted. + */ + fun moveProxy(proxyId: Int, aabb: AABB, displacement: Vec2): Boolean + + fun getUserData(proxyId: Int): Any? + + fun getFatAABB(proxyId: Int): AABB + + /** + * Query an AABB for overlapping proxies. The callback class is called for each proxy that + * overlaps the supplied AABB. + * + * @param callback + * @param araabbgAABB + */ + fun query(callback: TreeCallback, aabb: AABB) + + /** + * Ray-cast against the proxies in the tree. This relies on the callback to perform a exact + * ray-cast in the case were the proxy contains a shape. The callback also performs the any + * collision filtering. This has performance roughly equal to k * log(n), where k is the number of + * collisions and n is the number of proxies in the tree. + * + * @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). + * @param callback a callback class that is called for each proxy that is hit by the ray. + */ + fun raycast(callback: TreeRayCastCallback, input: RayCastInput) + + /** + * Compute the height of the tree. + */ + fun computeHeight(): Int + + fun drawTree(draw: DebugDraw) +} \ No newline at end of file diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DefaultBroadPhaseBuffer.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DefaultBroadPhaseBuffer.kt new file mode 100644 index 0000000..49ed4e7 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DefaultBroadPhaseBuffer.kt @@ -0,0 +1,235 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.broadphase + +import org.jbox2d.callbacks.DebugDraw +import org.jbox2d.callbacks.PairCallback +import org.jbox2d.callbacks.TreeCallback +import org.jbox2d.callbacks.TreeRayCastCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.Arrays_sort +import org.jbox2d.internal.arraycopy + +/** + * The broad-phase is used for computing pairs and performing volume queries and ray casts. This + * broad-phase does not persist pairs. Instead, this reports potentially new pairs. It is up to the + * client to consume the new pairs and to track subsequent overlap. + * + * @author Daniel Murphy + */ +class DefaultBroadPhaseBuffer(private val m_tree: BroadPhaseStrategy) : TreeCallback, BroadPhase { + + override var proxyCount: Int = 0 + private set + + private var m_moveBuffer: IntArray? = null + private var m_moveCapacity: Int = 0 + private var m_moveCount: Int = 0 + + private var m_pairBuffer: LongArray? = null + private var m_pairCapacity: Int = 0 + private var m_pairCount: Int = 0 + + private var m_queryProxyId: Int = 0 + + override val treeHeight: Int + get() = m_tree.height + + override val treeBalance: Int + get() = m_tree.maxBalance + + override val treeQuality: Float + get() = m_tree.areaRatio + + init { + proxyCount = 0 + + m_pairCapacity = 16 + m_pairCount = 0 + m_pairBuffer = LongArray(m_pairCapacity) + + m_moveCapacity = 16 + m_moveCount = 0 + m_moveBuffer = IntArray(m_moveCapacity) + m_queryProxyId = BroadPhase.NULL_PROXY + } + + override fun createProxy(aabb: AABB, userData: Any): Int { + val proxyId = m_tree.createProxy(aabb, userData) + ++proxyCount + bufferMove(proxyId) + return proxyId + } + + override fun destroyProxy(proxyId: Int) { + unbufferMove(proxyId) + --proxyCount + m_tree.destroyProxy(proxyId) + } + + override fun moveProxy(proxyId: Int, aabb: AABB, displacement: Vec2) { + val buffer = m_tree.moveProxy(proxyId, aabb, displacement) + if (buffer) { + bufferMove(proxyId) + } + } + + override fun touchProxy(proxyId: Int) { + bufferMove(proxyId) + } + + override fun getUserData(proxyId: Int): Any? { + return m_tree.getUserData(proxyId) + } + + override fun getFatAABB(proxyId: Int): AABB { + return m_tree.getFatAABB(proxyId) + } + + override fun testOverlap(proxyIdA: Int, proxyIdB: Int): Boolean { + // return AABB.testOverlap(proxyA.aabb, proxyB.aabb); + // return m_tree.overlap(proxyIdA, proxyIdB); + val a = m_tree.getFatAABB(proxyIdA) + val b = m_tree.getFatAABB(proxyIdB) + if (b.lowerBound.x - a.upperBound.x > 0.0f || b.lowerBound.y - a.upperBound.y > 0.0f) { + return false + } + + return !(a.lowerBound.x - b.upperBound.x > 0.0f || a.lowerBound.y - b.upperBound.y > 0.0f) + + } + + override fun drawTree(argDraw: DebugDraw) { + m_tree.drawTree(argDraw) + } + + override fun updatePairs(callback: PairCallback) { + // Reset pair buffer + m_pairCount = 0 + + // Perform tree queries for all moving proxies. + for (i in 0 until m_moveCount) { + m_queryProxyId = m_moveBuffer!![i] + if (m_queryProxyId == BroadPhase.NULL_PROXY) { + continue + } + + // We have to query the tree with the fat AABB so that + // we don't fail to create a pair that may touch later. + val fatAABB = m_tree.getFatAABB(m_queryProxyId) + + // Query tree, create pairs and add them pair buffer. + // log.debug("quering aabb: "+m_queryProxy.aabb); + m_tree.query(this, fatAABB) + } + // log.debug("Number of pairs found: "+m_pairCount); + + // Reset move buffer + m_moveCount = 0 + + // Sort the pair buffer to expose duplicates. + Arrays_sort(m_pairBuffer!!, 0, m_pairCount) + + // Send the pairs back to the client. + var i = 0 + while (i < m_pairCount) { + val primaryPair = m_pairBuffer!![i] + val userDataA = m_tree.getUserData((primaryPair shr 32).toInt()) + val userDataB = m_tree.getUserData(primaryPair.toInt()) + + // log.debug("returning pair: "+userDataA+", "+userDataB); + callback.addPair(userDataA, userDataB) + ++i + + // Skip any duplicate pairs. + while (i < m_pairCount) { + val pair = m_pairBuffer!![i] + if (pair != primaryPair) { + break + } + ++i + } + } + } + + override fun query(callback: TreeCallback, aabb: AABB) { + m_tree.query(callback, aabb) + } + + override fun raycast(callback: TreeRayCastCallback, input: RayCastInput) { + m_tree.raycast(callback, input) + } + + protected fun bufferMove(proxyId: Int) { + if (m_moveCount == m_moveCapacity) { + val old = m_moveBuffer + m_moveCapacity *= 2 + m_moveBuffer = IntArray(m_moveCapacity) + arraycopy(old!!, 0, m_moveBuffer!!, 0, old.size) + } + + m_moveBuffer!![m_moveCount] = proxyId + ++m_moveCount + } + + protected fun unbufferMove(proxyId: Int) { + for (i in 0 until m_moveCount) { + if (m_moveBuffer!![i] == proxyId) { + m_moveBuffer!![i] = BroadPhase.NULL_PROXY + } + } + } + + /** + * This is called from DynamicTree::query when we are gathering pairs. + */ + override fun treeCallback(proxyId: Int): Boolean { + // A proxy cannot form a pair with itself. + if (proxyId == m_queryProxyId) { + return true + } + + // Grow the pair buffer as needed. + if (m_pairCount == m_pairCapacity) { + val oldBuffer = m_pairBuffer + m_pairCapacity *= 2 + m_pairBuffer = LongArray(m_pairCapacity) + arraycopy(oldBuffer!!, 0, m_pairBuffer!!, 0, oldBuffer.size) + for (i in oldBuffer.size until m_pairCapacity) { + m_pairBuffer!![i] = 0 + } + } + + if (proxyId < m_queryProxyId) { + m_pairBuffer!![m_pairCount] = (proxyId.toLong() shl 32) or m_queryProxyId.toLong() + } else { + m_pairBuffer!![m_pairCount] = (m_queryProxyId.toLong() shl 32) or proxyId.toLong() + } + + ++m_pairCount + return true + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTree.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTree.kt new file mode 100644 index 0000000..cc60e70 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTree.kt @@ -0,0 +1,871 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.broadphase + +import org.jbox2d.callbacks.DebugDraw +import org.jbox2d.callbacks.TreeCallback +import org.jbox2d.callbacks.TreeRayCastCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.common.Color3f +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + +/** + * A dynamic tree arranges data in a binary tree to accelerate queries such as volume queries and + * ray casts. Leafs are proxies with an AABB. In the tree we expand the proxy AABB by _fatAABBFactor + * so that the proxy AABB is bigger than the client object. This allows the client object to move by + * small amounts without triggering a tree update. + * + * @author daniel + */ +class DynamicTree : BroadPhaseStrategy { + + private var m_root: DynamicTreeNode? = null + private var m_nodes: Array = Array(16) { DynamicTreeNode(it) } + private var m_nodeCount: Int = 0 + private var m_nodeCapacity: Int = 16 + + private var m_freeList: Int = 0 + + private val drawVecs = Array(4) { Vec2() } + private var nodeStack = arrayOfNulls(20) + private var nodeStackIndex = 0 + + private val r = Vec2() + private val aabb = AABB() + private val subInput = RayCastInput() + + override val height: Int + get() = if (m_root == null) { + 0 + } else m_root!!.height + + override val maxBalance: Int + get() { + var maxBalance = 0 + for (i in 0 until m_nodeCapacity) { + val node = m_nodes!![i] + if (node.height <= 1) { + continue + } + + assert(!(node.child1 == null)) + + val child1 = node.child1 + val child2 = node.child2 + val balance = MathUtils.abs(child2!!.height - child1!!.height) + maxBalance = MathUtils.max(maxBalance, balance) + } + + return maxBalance + } + + override// Free node in pool + val areaRatio: Float + get() { + if (m_root == null) { + return 0.0f + } + + val root = m_root + val rootArea = root!!.aabb.perimeter + + var totalArea = 0.0f + for (i in 0 until m_nodeCapacity) { + val node = m_nodes!![i] + if (node.height < 0) { + continue + } + + totalArea += node.aabb.perimeter + } + + return totalArea / rootArea + } + + private val combinedAABB = AABB() + + private val color = Color3f() + private val textVec = Vec2() + + init { + // Build a linked list for the free list. + for (i in m_nodeCapacity - 1 downTo 0) { + m_nodes!![i].parent = if (i == m_nodeCapacity - 1) null else m_nodes!![i + 1] + m_nodes!![i].height = -1 + } + } + + override fun createProxy(aabb: AABB, userData: Any): Int { + assert(aabb.isValid) + val node = allocateNode() + val proxyId = node.id + // Fatten the aabb + val nodeAABB = node.aabb + nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension + nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension + nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension + nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension + node.userData = userData + + insertLeaf(proxyId) + + return proxyId + } + + override fun destroyProxy(proxyId: Int) { + assert(0 <= proxyId && proxyId < m_nodeCapacity) + val node = m_nodes!![proxyId] + assert(node.child1 == null) + + removeLeaf(node) + freeNode(node) + } + + override fun moveProxy(proxyId: Int, aabb: AABB, displacement: Vec2): Boolean { + assert(aabb.isValid) + assert(0 <= proxyId && proxyId < m_nodeCapacity) + val node = m_nodes!![proxyId] + assert(node.child1 == null) + + val nodeAABB = node.aabb + // if (nodeAABB.contains(aabb)) { + if (nodeAABB.lowerBound.x <= aabb.lowerBound.x && nodeAABB.lowerBound.y <= aabb.lowerBound.y + && aabb.upperBound.x <= nodeAABB.upperBound.x && aabb.upperBound.y <= nodeAABB.upperBound.y) { + return false + } + + removeLeaf(node) + + // Extend AABB + val lowerBound = nodeAABB.lowerBound + val upperBound = nodeAABB.upperBound + lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension + lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension + upperBound.x = aabb.upperBound.x + Settings.aabbExtension + upperBound.y = aabb.upperBound.y + Settings.aabbExtension + + // Predict AABB displacement. + val dx = displacement.x * Settings.aabbMultiplier + val dy = displacement.y * Settings.aabbMultiplier + if (dx < 0.0f) { + lowerBound.x += dx + } else { + upperBound.x += dx + } + + if (dy < 0.0f) { + lowerBound.y += dy + } else { + upperBound.y += dy + } + + insertLeaf(proxyId) + return true + } + + override fun getUserData(proxyId: Int): Any? { + assert(0 <= proxyId && proxyId < m_nodeCapacity) + return m_nodes!![proxyId].userData + } + + override fun getFatAABB(proxyId: Int): AABB { + assert(0 <= proxyId && proxyId < m_nodeCapacity) + return m_nodes!![proxyId].aabb + } + + override fun query(callback: TreeCallback, aabb: AABB) { + assert(aabb.isValid) + nodeStackIndex = 0 + nodeStack[nodeStackIndex++] = m_root + + while (nodeStackIndex > 0) { + val node = nodeStack[--nodeStackIndex] ?: continue + + if (AABB.testOverlap(node.aabb, aabb)) { + if (node.child1 == null) { + val proceed = callback.treeCallback(node.id) + if (!proceed) { + return + } + } else { + if (nodeStack.size - nodeStackIndex - 2 <= 0) { + val newBuffer = arrayOfNulls(nodeStack.size * 2) + arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.size) + nodeStack = newBuffer + } + nodeStack[nodeStackIndex++] = node.child1 + nodeStack[nodeStackIndex++] = node.child2 + } + } + } + } + + override fun raycast(callback: TreeRayCastCallback, input: RayCastInput) { + val p1 = input.p1 + val p2 = input.p2 + val p1x = p1.x + val p2x = p2.x + val p1y = p1.y + val p2y = p2.y + val vx: Float + val vy: Float + val rx: Float + val ry: Float + val absVx: Float + val absVy: Float + var cx: Float + var cy: Float + var hx: Float + var hy: Float + var tempx: Float + var tempy: Float + r.x = p2x - p1x + r.y = p2y - p1y + assert(r.x * r.x + r.y * r.y > 0f) + r.normalize() + rx = r.x + ry = r.y + + // v is perpendicular to the segment. + vx = -1f * ry + vy = 1f * rx + absVx = MathUtils.abs(vx) + absVy = MathUtils.abs(vy) + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + var maxFraction = input.maxFraction + + // Build a bounding box for the segment. + val segAABB = aabb + // Vec2 t = p1 + maxFraction * (p2 - p1); + // before inline + // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); + // Vec2.minToOut(p1, temp, segAABB.lowerBound); + // Vec2.maxToOut(p1, temp, segAABB.upperBound); + tempx = (p2x - p1x) * maxFraction + p1x + tempy = (p2y - p1y) * maxFraction + p1y + segAABB.lowerBound.x = if (p1x < tempx) p1x else tempx + segAABB.lowerBound.y = if (p1y < tempy) p1y else tempy + segAABB.upperBound.x = if (p1x > tempx) p1x else tempx + segAABB.upperBound.y = if (p1y > tempy) p1y else tempy + // end inline + + nodeStackIndex = 0 + nodeStack[nodeStackIndex++] = m_root + while (nodeStackIndex > 0) { + val node = nodeStack[--nodeStackIndex] ?: continue + + val nodeAABB = node.aabb + if (!AABB.testOverlap(nodeAABB, segAABB)) { + continue + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // node.aabb.getCenterToOut(c); + // node.aabb.getExtentsToOut(h); + cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f + cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f + hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f + hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f + tempx = p1x - cx + tempy = p1y - cy + val separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy) + if (separation > 0.0f) { + continue + } + + if (node.child1 == null) { + subInput.p1.x = p1x + subInput.p1.y = p1y + subInput.p2.x = p2x + subInput.p2.y = p2y + subInput.maxFraction = maxFraction + + val value = callback.raycastCallback(subInput, node.id) + + if (value == 0.0f) { + // The client has terminated the ray cast. + return + } + + if (value > 0.0f) { + // Update segment bounding box. + maxFraction = value + // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); + // Vec2.minToOut(p1, temp, segAABB.lowerBound); + // Vec2.maxToOut(p1, temp, segAABB.upperBound); + tempx = (p2x - p1x) * maxFraction + p1x + tempy = (p2y - p1y) * maxFraction + p1y + segAABB.lowerBound.x = if (p1x < tempx) p1x else tempx + segAABB.lowerBound.y = if (p1y < tempy) p1y else tempy + segAABB.upperBound.x = if (p1x > tempx) p1x else tempx + segAABB.upperBound.y = if (p1y > tempy) p1y else tempy + } + } else { + if (nodeStack.size - nodeStackIndex - 2 <= 0) { + val newBuffer = arrayOfNulls(nodeStack.size * 2) + arraycopy(nodeStack, 0, newBuffer, 0, nodeStack.size) + nodeStack = newBuffer + } + nodeStack[nodeStackIndex++] = node.child1 + nodeStack[nodeStackIndex++] = node.child2 + } + } + } + + override fun computeHeight(): Int { + return computeHeight(m_root!!) + } + + private fun computeHeight(node: DynamicTreeNode): Int { + assert(0 <= node.id && node.id < m_nodeCapacity) + + if (node.child1 == null) { + return 0 + } + val height1 = computeHeight(node.child1!!) + val height2 = computeHeight(node.child2!!) + return 1 + MathUtils.max(height1, height2) + } + + /** + * Validate this tree. For testing. + */ + fun validate() { + validateStructure(m_root) + validateMetrics(m_root) + + var freeCount = 0 + var freeNode: DynamicTreeNode? = if (m_freeList != NULL_NODE) m_nodes!![m_freeList] else null + while (freeNode != null) { + assert(0 <= freeNode.id && freeNode.id < m_nodeCapacity) + assert(freeNode === m_nodes!![freeNode.id]) + freeNode = freeNode.parent + ++freeCount + } + + assert(height == computeHeight()) + + assert(m_nodeCount + freeCount == m_nodeCapacity) + } + + /** + * Build an optimal tree. Very expensive. For testing. + */ + fun rebuildBottomUp() { + val nodes = IntArray(m_nodeCount) + var count = 0 + + // Build array of leaves. Free the rest. + for (i in 0 until m_nodeCapacity) { + if (m_nodes!![i].height < 0) { + // free node in pool + continue + } + + val node = m_nodes!![i] + if (node.child1 == null) { + node.parent = null + nodes[count] = i + ++count + } else { + freeNode(node) + } + } + + val b = AABB() + while (count > 1) { + var minCost = Float.MAX_VALUE + var iMin = -1 + var jMin = -1 + for (i in 0 until count) { + val aabbi = m_nodes!![nodes[i]].aabb + + for (j in i + 1 until count) { + val aabbj = m_nodes!![nodes[j]].aabb + b.combine(aabbi, aabbj) + val cost = b.perimeter + if (cost < minCost) { + iMin = i + jMin = j + minCost = cost + } + } + } + + val index1 = nodes[iMin] + val index2 = nodes[jMin] + val child1 = m_nodes!![index1] + val child2 = m_nodes!![index2] + + val parent = allocateNode() + parent.child1 = child1 + parent.child2 = child2 + parent.height = 1 + MathUtils.max(child1.height, child2.height) + parent.aabb.combine(child1.aabb, child2.aabb) + parent.parent = null + + child1.parent = parent + child2.parent = parent + + nodes[jMin] = nodes[count - 1] + nodes[iMin] = parent.id + --count + } + + m_root = m_nodes!![nodes[0]] + + validate() + } + + private fun allocateNode(): DynamicTreeNode { + if (m_freeList == NULL_NODE) { + assert(m_nodeCount == m_nodeCapacity) + + val old = m_nodes + m_nodeCapacity *= 2 + m_nodes = arrayOfNulls(m_nodeCapacity) as Array + arraycopy(old!!, 0, m_nodes!!, 0, old.size) + + // Build a linked list for the free list. + for (i in m_nodeCapacity - 1 downTo m_nodeCount) { + m_nodes!![i] = DynamicTreeNode(i) + m_nodes!![i].parent = if (i == m_nodeCapacity - 1) null else m_nodes!![i + 1] + m_nodes!![i].height = -1 + } + m_freeList = m_nodeCount + } + val nodeId = m_freeList + val treeNode = m_nodes!![nodeId] + m_freeList = if (treeNode.parent != null) treeNode.parent!!.id else NULL_NODE + + treeNode.parent = null + treeNode.child1 = null + treeNode.child2 = null + treeNode.height = 0 + treeNode.userData = null + ++m_nodeCount + return treeNode + } + + /** + * returns a node to the pool + */ + private fun freeNode(node: DynamicTreeNode) { + assert(node != null) + assert(0 < m_nodeCount) + node.parent = if (m_freeList != NULL_NODE) m_nodes!![m_freeList] else null + node.height = -1 + m_freeList = node.id + m_nodeCount-- + } + + private fun insertLeaf(leaf_index: Int) { + val leaf = m_nodes!![leaf_index] + if (m_root == null) { + m_root = leaf + m_root!!.parent = null + return + } + + // find the best sibling + val leafAABB = leaf.aabb + var index = m_root + while (index!!.child1 != null) { + val node = index + val child1 = node.child1 + val child2 = node.child2 + + val area = node.aabb.perimeter + + combinedAABB.combine(node.aabb, leafAABB) + val combinedArea = combinedAABB.perimeter + + // Cost of creating a new parent for this node and the new leaf + val cost = 2.0f * combinedArea + + // Minimum cost of pushing the leaf further down the tree + val inheritanceCost = 2.0f * (combinedArea - area) + + // Cost of descending into child1 + val cost1: Float + if (child1!!.child1 == null) { + combinedAABB.combine(leafAABB, child1.aabb) + cost1 = combinedAABB.perimeter + inheritanceCost + } else { + combinedAABB.combine(leafAABB, child1.aabb) + val oldArea = child1.aabb.perimeter + val newArea = combinedAABB.perimeter + cost1 = newArea - oldArea + inheritanceCost + } + + // Cost of descending into child2 + val cost2: Float + if (child2!!.child1 == null) { + combinedAABB.combine(leafAABB, child2.aabb) + cost2 = combinedAABB.perimeter + inheritanceCost + } else { + combinedAABB.combine(leafAABB, child2.aabb) + val oldArea = child2.aabb.perimeter + val newArea = combinedAABB.perimeter + cost2 = newArea - oldArea + inheritanceCost + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) { + break + } + + // Descend + if (cost1 < cost2) { + index = child1 + } else { + index = child2 + } + } + + val sibling = index + val oldParent = m_nodes!![sibling.id].parent + val newParent = allocateNode() + newParent.parent = oldParent + newParent.userData = null + newParent.aabb.combine(leafAABB, sibling.aabb) + newParent.height = sibling.height + 1 + + if (oldParent != null) { + // The sibling was not the root. + if (oldParent.child1 === sibling) { + oldParent.child1 = newParent + } else { + oldParent.child2 = newParent + } + + newParent.child1 = sibling + newParent.child2 = leaf + sibling.parent = newParent + leaf.parent = newParent + } else { + // The sibling was the root. + newParent.child1 = sibling + newParent.child2 = leaf + sibling.parent = newParent + leaf.parent = newParent + m_root = newParent + } + + // Walk back up the tree fixing heights and AABBs + index = leaf.parent + while (index != null) { + index = balance(index) + + val child1 = index.child1 + val child2 = index.child2 + + assert(child1 != null) + assert(child2 != null) + + index.height = 1 + MathUtils.max(child1!!.height, child2!!.height) + index.aabb.combine(child1.aabb, child2.aabb) + + index = index.parent + } + // validate(); + } + + private fun removeLeaf(leaf: DynamicTreeNode) { + if (leaf === m_root) { + m_root = null + return + } + + val parent = leaf.parent + val grandParent = parent?.parent + val sibling: DynamicTreeNode + if (parent!!.child1 === leaf) { + sibling = parent!!.child2!! + } else { + sibling = parent!!.child1!! + } + + if (grandParent != null) { + // Destroy parent and connect sibling to grandParent. + if (grandParent.child1 === parent) { + grandParent.child1 = sibling + } else { + grandParent.child2 = sibling + } + sibling.parent = grandParent + freeNode(parent) + + // Adjust ancestor bounds. + var index = grandParent + while (index != null) { + index = balance(index) + + val child1 = index.child1 + val child2 = index.child2 + + index.aabb.combine(child1!!.aabb, child2!!.aabb) + index.height = 1 + MathUtils.max(child1.height, child2.height) + + index = index.parent + } + } else { + m_root = sibling + sibling.parent = null + freeNode(parent) + } + + // validate(); + } + + // Perform a left or right rotation if node A is imbalanced. + // Returns the new root index. + private fun balance(iA: DynamicTreeNode?): DynamicTreeNode { + assert(iA != null) + + val A = iA + if (A!!.child1 == null || A.height < 2) { + return iA + } + + val iB = A.child1 + val iC = A.child2 + assert(0 <= iB!!.id && iB.id < m_nodeCapacity) + assert(0 <= iC!!.id && iC.id < m_nodeCapacity) + + val B = iB + val C = iC + + val balance = C.height - B.height + + // Rotate C up + if (balance > 1) { + val iF = C.child1 + val iG = C.child2 + val F = iF + val G = iG + assert(F != null) + assert(G != null) + assert(0 <= iF!!.id && iF.id < m_nodeCapacity) + assert(0 <= iG!!.id && iG.id < m_nodeCapacity) + + // Swap A and C + C.child1 = iA + C.parent = A.parent + A.parent = iC + + // A's old parent should point to C + if (C.parent != null) { + if (C.parent!!.child1 === iA) { + C.parent!!.child1 = iC + } else { + assert(C.parent!!.child2 === iA) + C.parent!!.child2 = iC + } + } else { + m_root = iC + } + + // Rotate + if (F!!.height > G!!.height) { + C.child2 = iF + A.child2 = iG + G.parent = iA + A.aabb.combine(B.aabb, G.aabb) + C.aabb.combine(A.aabb, F.aabb) + + A.height = 1 + MathUtils.max(B.height, G.height) + C.height = 1 + MathUtils.max(A.height, F.height) + } else { + C.child2 = iG + A.child2 = iF + F.parent = iA + A.aabb.combine(B.aabb, F.aabb) + C.aabb.combine(A.aabb, G.aabb) + + A.height = 1 + MathUtils.max(B.height, F.height) + C.height = 1 + MathUtils.max(A.height, G.height) + } + + return iC + } + + // Rotate B up + if (balance < -1) { + val iD = B.child1 + val iE = B.child2 + val D = iD + val E = iE + assert(0 <= iD!!.id && iD.id < m_nodeCapacity) + assert(0 <= iE!!.id && iE.id < m_nodeCapacity) + + // Swap A and B + B.child1 = iA + B.parent = A.parent + A.parent = iB + + // A's old parent should point to B + if (B.parent != null) { + if (B.parent!!.child1 === iA) { + B.parent!!.child1 = iB + } else { + assert(B.parent!!.child2 === iA) + B.parent!!.child2 = iB + } + } else { + m_root = iB + } + + // Rotate + if (D!!.height > E!!.height) { + B.child2 = iD + A.child1 = iE + E.parent = iA + A.aabb.combine(C.aabb, E.aabb) + B.aabb.combine(A.aabb, D.aabb) + + A.height = 1 + MathUtils.max(C.height, E.height) + B.height = 1 + MathUtils.max(A.height, D.height) + } else { + B.child2 = iE + A.child1 = iD + D.parent = iA + A.aabb.combine(C.aabb, D.aabb) + B.aabb.combine(A.aabb, E.aabb) + + A.height = 1 + MathUtils.max(C.height, D.height) + B.height = 1 + MathUtils.max(A.height, E.height) + } + + return iB + } + + return iA + } + + private fun validateStructure(node: DynamicTreeNode?) { + if (node == null) { + return + } + assert(node === m_nodes!![node.id]) + + if (node === m_root) { + assert(node.parent == null) + } + + val child1 = node.child1 + val child2 = node.child2 + + if (node.child1 == null) { + assert(child1 == null) + assert(child2 == null) + assert(node.height == 0) + return + } + + assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity) + assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity) + + assert(child1!!.parent === node) + assert(child2!!.parent === node) + + validateStructure(child1) + validateStructure(child2) + } + + private fun validateMetrics(node: DynamicTreeNode?) { + if (node == null) { + return + } + + val child1 = node.child1 + val child2 = node.child2 + + if (node.child1 == null) { + assert(child1 == null) + assert(child2 == null) + assert(node.height == 0) + return + } + + assert(child1 != null && 0 <= child1.id && child1.id < m_nodeCapacity) + assert(child2 != null && 0 <= child2.id && child2.id < m_nodeCapacity) + + val height1 = child1!!.height + val height2 = child2!!.height + val height: Int + height = 1 + MathUtils.max(height1, height2) + assert(node.height == height) + + val aabb = AABB() + aabb.combine(child1.aabb, child2.aabb) + + assert(aabb.lowerBound == node.aabb.lowerBound) + assert(aabb.upperBound == node.aabb.upperBound) + + validateMetrics(child1) + validateMetrics(child2) + } + + override fun drawTree(argDraw: DebugDraw) { + if (m_root == null) { + return + } + val height = computeHeight() + drawTree(argDraw, m_root!!, 0, height) + } + + fun drawTree(argDraw: DebugDraw, node: DynamicTreeNode, spot: Int, height: Int) { + node.aabb.getVertices(drawVecs!!) + + color.set(1f, (height - spot) * 1f / height, (height - spot) * 1f / height) + argDraw.drawPolygon(drawVecs!! as Array, 4, color) + + argDraw.viewportTranform!!.getWorldToScreen(node.aabb.upperBound, textVec) + argDraw.drawString(textVec.x, textVec.y, node.id.toString() + "-" + (spot + 1) + "/" + height, color) + + if (node.child1 != null) { + drawTree(argDraw, node.child1!!, spot + 1, height) + } + if (node.child2 != null) { + drawTree(argDraw, node.child2!!, spot + 1, height) + } + } + + companion object { + + val MAX_STACK_SIZE = 64 + + val NULL_NODE = -1 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeFlatNodes.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeFlatNodes.kt new file mode 100644 index 0000000..6498e97 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeFlatNodes.kt @@ -0,0 +1,873 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.broadphase + +import org.jbox2d.callbacks.DebugDraw +import org.jbox2d.callbacks.TreeCallback +import org.jbox2d.callbacks.TreeRayCastCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.common.BufferUtils +import org.jbox2d.common.Color3f +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert + +class DynamicTreeFlatNodes : BroadPhaseStrategy { + + + var m_root: Int = NULL_NODE + lateinit var m_aabb: Array + lateinit var m_userData: Array + lateinit protected var m_parent: IntArray + lateinit protected var m_child1: IntArray + lateinit protected var m_child2: IntArray + lateinit protected var m_height: IntArray + + private var m_nodeCount: Int = 0 + private var m_nodeCapacity: Int = 16 + + private var m_freeList: Int = 0 + + private val drawVecs = Array(4) { Vec2() } + + private var nodeStack = IntArray(20) + private var nodeStackIndex: Int = 0 + + private val r = Vec2() + private val aabb = AABB() + private val subInput = RayCastInput() + + override val height: Int + get() = if (m_root == NULL_NODE) { + 0 + } else m_height[m_root] + + override val maxBalance: Int + get() { + var maxBalance = 0 + for (i in 0 until m_nodeCapacity) { + if (m_height[i] <= 1) { + continue + } + + assert(m_child1[i] != NULL_NODE) + + val child1 = m_child1[i] + val child2 = m_child2[i] + val balance = MathUtils.abs(m_height[child2] - m_height[child1]) + maxBalance = MathUtils.max(maxBalance, balance) + } + + return maxBalance + } + + override// Free node in pool + val areaRatio: Float + get() { + if (m_root == NULL_NODE) { + return 0.0f + } + + val root = m_root + val rootArea = m_aabb[root].perimeter + + var totalArea = 0.0f + for (i in 0 until m_nodeCapacity) { + if (m_height[i] < 0) { + continue + } + + totalArea += m_aabb[i].perimeter + } + + return totalArea / rootArea + } + + private val combinedAABB = AABB() + + private val color = Color3f() + private val textVec = Vec2() + + init { + expandBuffers(0, m_nodeCapacity) + } + + private fun expandBuffers(oldSize: Int, newSize: Int) { + m_aabb = BufferUtils.reallocateBuffer({ AABB() }, m_aabb, oldSize, newSize) + m_userData = BufferUtils.reallocateBuffer({ Any() }, m_userData as Array, oldSize, newSize) as Array + m_parent = BufferUtils.reallocateBuffer(m_parent, oldSize, newSize) + m_child1 = BufferUtils.reallocateBuffer(m_child1, oldSize, newSize) + m_child2 = BufferUtils.reallocateBuffer(m_child2, oldSize, newSize) + m_height = BufferUtils.reallocateBuffer(m_height, oldSize, newSize) + + // Build a linked list for the free list. + for (i in oldSize until newSize) { + m_aabb[i] = AABB() + m_parent[i] = if (i == newSize - 1) NULL_NODE else i + 1 + m_height[i] = -1 + m_child1[i] = -1 + m_child2[i] = -1 + } + m_freeList = oldSize + } + + override fun createProxy(aabb: AABB, userData: Any): Int { + val node = allocateNode() + // Fatten the aabb + val nodeAABB = m_aabb[node] + nodeAABB.lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension + nodeAABB.lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension + nodeAABB.upperBound.x = aabb.upperBound.x + Settings.aabbExtension + nodeAABB.upperBound.y = aabb.upperBound.y + Settings.aabbExtension + m_userData[node] = userData + + insertLeaf(node) + + return node + } + + override fun destroyProxy(proxyId: Int) { + assert(0 <= proxyId && proxyId < m_nodeCapacity) + assert(m_child1[proxyId] == NULL_NODE) + + removeLeaf(proxyId) + freeNode(proxyId) + } + + override fun moveProxy(proxyId: Int, aabb: AABB, displacement: Vec2): Boolean { + assert(0 <= proxyId && proxyId < m_nodeCapacity) + val node = proxyId + assert(m_child1[node] == NULL_NODE) + + val nodeAABB = m_aabb[node] + // if (nodeAABB.contains(aabb)) { + if (nodeAABB.lowerBound.x <= aabb.lowerBound.x && nodeAABB.lowerBound.y <= aabb.lowerBound.y + && aabb.upperBound.x <= nodeAABB.upperBound.x && aabb.upperBound.y <= nodeAABB.upperBound.y) { + return false + } + + removeLeaf(node) + + // Extend AABB + val lowerBound = nodeAABB.lowerBound + val upperBound = nodeAABB.upperBound + lowerBound.x = aabb.lowerBound.x - Settings.aabbExtension + lowerBound.y = aabb.lowerBound.y - Settings.aabbExtension + upperBound.x = aabb.upperBound.x + Settings.aabbExtension + upperBound.y = aabb.upperBound.y + Settings.aabbExtension + + // Predict AABB displacement. + val dx = displacement.x * Settings.aabbMultiplier + val dy = displacement.y * Settings.aabbMultiplier + if (dx < 0.0f) { + lowerBound.x += dx + } else { + upperBound.x += dx + } + + if (dy < 0.0f) { + lowerBound.y += dy + } else { + upperBound.y += dy + } + + insertLeaf(proxyId) + return true + } + + override fun getUserData(proxyId: Int): Any? { + assert(0 <= proxyId && proxyId < m_nodeCount) + return m_userData[proxyId] + } + + override fun getFatAABB(proxyId: Int): AABB { + assert(0 <= proxyId && proxyId < m_nodeCount) + return m_aabb[proxyId] + } + + override fun query(callback: TreeCallback, aabb: AABB) { + nodeStackIndex = 0 + nodeStack[nodeStackIndex++] = m_root + + while (nodeStackIndex > 0) { + val node = nodeStack[--nodeStackIndex] + if (node == NULL_NODE) { + continue + } + + if (AABB.testOverlap(m_aabb[node], aabb)) { + val child1 = m_child1[node] + if (child1 == NULL_NODE) { + val proceed = callback.treeCallback(node) + if (!proceed) { + return + } + } else { + if (nodeStack.size - nodeStackIndex - 2 <= 0) { + nodeStack = BufferUtils.reallocateBuffer(nodeStack, nodeStack.size, nodeStack.size * 2) + } + nodeStack[nodeStackIndex++] = child1 + nodeStack[nodeStackIndex++] = m_child2[node] + } + } + } + } + + override fun raycast(callback: TreeRayCastCallback, input: RayCastInput) { + val p1 = input.p1 + val p2 = input.p2 + val p1x = p1.x + val p2x = p2.x + val p1y = p1.y + val p2y = p2.y + val vx: Float + val vy: Float + val rx: Float + val ry: Float + val absVx: Float + val absVy: Float + var cx: Float + var cy: Float + var hx: Float + var hy: Float + var tempx: Float + var tempy: Float + r.x = p2x - p1x + r.y = p2y - p1y + assert(r.x * r.x + r.y * r.y > 0f) + r.normalize() + rx = r.x + ry = r.y + + // v is perpendicular to the segment. + vx = -1f * ry + vy = 1f * rx + absVx = MathUtils.abs(vx) + absVy = MathUtils.abs(vy) + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + var maxFraction = input.maxFraction + + // Build a bounding box for the segment. + val segAABB = aabb + // Vec2 t = p1 + maxFraction * (p2 - p1); + // before inline + // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); + // Vec2.minToOut(p1, temp, segAABB.lowerBound); + // Vec2.maxToOut(p1, temp, segAABB.upperBound); + tempx = (p2x - p1x) * maxFraction + p1x + tempy = (p2y - p1y) * maxFraction + p1y + segAABB.lowerBound.x = if (p1x < tempx) p1x else tempx + segAABB.lowerBound.y = if (p1y < tempy) p1y else tempy + segAABB.upperBound.x = if (p1x > tempx) p1x else tempx + segAABB.upperBound.y = if (p1y > tempy) p1y else tempy + // end inline + + nodeStackIndex = 0 + nodeStack[nodeStackIndex++] = m_root + while (nodeStackIndex > 0) { + nodeStack[--nodeStackIndex] = m_root + val node = nodeStack[--nodeStackIndex] + if (node == NULL_NODE) { + continue + } + + val nodeAABB = m_aabb[node] + if (!AABB.testOverlap(nodeAABB, segAABB)) { + continue + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // node.aabb.getCenterToOut(c); + // node.aabb.getExtentsToOut(h); + cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x) * .5f + cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y) * .5f + hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x) * .5f + hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y) * .5f + tempx = p1x - cx + tempy = p1y - cy + val separation = MathUtils.abs(vx * tempx + vy * tempy) - (absVx * hx + absVy * hy) + if (separation > 0.0f) { + continue + } + + val child1 = m_child1[node] + if (child1 == NULL_NODE) { + subInput.p1.x = p1x + subInput.p1.y = p1y + subInput.p2.x = p2x + subInput.p2.y = p2y + subInput.maxFraction = maxFraction + + val value = callback.raycastCallback(subInput, node) + + if (value == 0.0f) { + // The client has terminated the ray cast. + return + } + + if (value > 0.0f) { + // Update segment bounding box. + maxFraction = value + // temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1); + // Vec2.minToOut(p1, temp, segAABB.lowerBound); + // Vec2.maxToOut(p1, temp, segAABB.upperBound); + tempx = (p2x - p1x) * maxFraction + p1x + tempy = (p2y - p1y) * maxFraction + p1y + segAABB.lowerBound.x = if (p1x < tempx) p1x else tempx + segAABB.lowerBound.y = if (p1y < tempy) p1y else tempy + segAABB.upperBound.x = if (p1x > tempx) p1x else tempx + segAABB.upperBound.y = if (p1y > tempy) p1y else tempy + } + } else { + nodeStack[nodeStackIndex++] = child1 + nodeStack[nodeStackIndex++] = m_child2[node] + } + } + } + + override fun computeHeight(): Int { + return computeHeight(m_root) + } + + private fun computeHeight(node: Int): Int { + assert(0 <= node && node < m_nodeCapacity) + + if (m_child1[node] == NULL_NODE) { + return 0 + } + val height1 = computeHeight(m_child1[node]) + val height2 = computeHeight(m_child2[node]) + return 1 + MathUtils.max(height1, height2) + } + + /** + * Validate this tree. For testing. + */ + fun validate() { + validateStructure(m_root) + validateMetrics(m_root) + + var freeCount = 0 + var freeNode = m_freeList + while (freeNode != NULL_NODE) { + assert(0 <= freeNode && freeNode < m_nodeCapacity) + freeNode = m_parent[freeNode] + ++freeCount + } + + assert(height == computeHeight()) + assert(m_nodeCount + freeCount == m_nodeCapacity) + } + + // /** + // * Build an optimal tree. Very expensive. For testing. + // */ + // public void rebuildBottomUp() { + // int[] nodes = new int[m_nodeCount]; + // int count = 0; + // + // // Build array of leaves. Free the rest. + // for (int i = 0; i < m_nodeCapacity; ++i) { + // if (m_nodes[i].height < 0) { + // // free node in pool + // continue; + // } + // + // DynamicTreeNode node = m_nodes[i]; + // if (node.isLeaf()) { + // node.parent = null; + // nodes[count] = i; + // ++count; + // } else { + // freeNode(node); + // } + // } + // + // AABB b = new AABB(); + // while (count > 1) { + // float minCost = Float.MAX_VALUE; + // int iMin = -1, jMin = -1; + // for (int i = 0; i < count; ++i) { + // AABB aabbi = m_nodes[nodes[i]].aabb; + // + // for (int j = i + 1; j < count; ++j) { + // AABB aabbj = m_nodes[nodes[j]].aabb; + // b.combine(aabbi, aabbj); + // float cost = b.getPerimeter(); + // if (cost < minCost) { + // iMin = i; + // jMin = j; + // minCost = cost; + // } + // } + // } + // + // int index1 = nodes[iMin]; + // int index2 = nodes[jMin]; + // DynamicTreeNode child1 = m_nodes[index1]; + // DynamicTreeNode child2 = m_nodes[index2]; + // + // DynamicTreeNode parent = allocateNode(); + // parent.child1 = child1; + // parent.child2 = child2; + // parent.height = 1 + MathUtils.max(child1.height, child2.height); + // parent.aabb.combine(child1.aabb, child2.aabb); + // parent.parent = null; + // + // child1.parent = parent; + // child2.parent = parent; + // + // nodes[jMin] = nodes[count - 1]; + // nodes[iMin] = parent.id; + // --count; + // } + // + // m_root = m_nodes[nodes[0]]; + // + // validate(); + // } + + private fun allocateNode(): Int { + if (m_freeList == NULL_NODE) { + assert(m_nodeCount == m_nodeCapacity) + m_nodeCapacity *= 2 + expandBuffers(m_nodeCount, m_nodeCapacity) + } + assert(m_freeList != NULL_NODE) + val node = m_freeList + m_freeList = m_parent[node] + m_parent[node] = NULL_NODE + m_child1[node] = NULL_NODE + m_height[node] = 0 + ++m_nodeCount + return node + } + + /** + * returns a node to the pool + */ + private fun freeNode(node: Int) { + assert(node != NULL_NODE) + assert(0 < m_nodeCount) + m_parent[node] = if (m_freeList != NULL_NODE) m_freeList else NULL_NODE + m_height[node] = -1 + m_freeList = node + m_nodeCount-- + } + + private fun insertLeaf(leaf: Int) { + if (m_root == NULL_NODE) { + m_root = leaf + m_parent[m_root] = NULL_NODE + return + } + + // find the best sibling + val leafAABB = m_aabb[leaf] + var index = m_root + while (m_child1[index] != NULL_NODE) { + val node = index + val child1 = m_child1[node] + val child2 = m_child2[node] + val nodeAABB = m_aabb[node] + val area = nodeAABB.perimeter + + combinedAABB.combine(nodeAABB, leafAABB) + val combinedArea = combinedAABB.perimeter + + // Cost of creating a new parent for this node and the new leaf + val cost = 2.0f * combinedArea + + // Minimum cost of pushing the leaf further down the tree + val inheritanceCost = 2.0f * (combinedArea - area) + + // Cost of descending into child1 + val cost1: Float + val child1AABB = m_aabb[child1] + if (m_child1[child1] == NULL_NODE) { + combinedAABB.combine(leafAABB, child1AABB) + cost1 = combinedAABB.perimeter + inheritanceCost + } else { + combinedAABB.combine(leafAABB, child1AABB) + val oldArea = child1AABB.perimeter + val newArea = combinedAABB.perimeter + cost1 = newArea - oldArea + inheritanceCost + } + + // Cost of descending into child2 + val cost2: Float + val child2AABB = m_aabb[child2] + if (m_child1[child2] == NULL_NODE) { + combinedAABB.combine(leafAABB, child2AABB) + cost2 = combinedAABB.perimeter + inheritanceCost + } else { + combinedAABB.combine(leafAABB, child2AABB) + val oldArea = child2AABB.perimeter + val newArea = combinedAABB.perimeter + cost2 = newArea - oldArea + inheritanceCost + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) { + break + } + + // Descend + if (cost1 < cost2) { + index = child1 + } else { + index = child2 + } + } + + val sibling = index + val oldParent = m_parent[sibling] + val newParent = allocateNode() + m_parent[newParent] = oldParent + m_userData[newParent] = null + m_aabb[newParent].combine(leafAABB, m_aabb[sibling]) + m_height[newParent] = m_height[sibling] + 1 + + if (oldParent != NULL_NODE) { + // The sibling was not the root. + if (m_child1[oldParent] == sibling) { + m_child1[oldParent] = newParent + } else { + m_child2[oldParent] = newParent + } + + m_child1[newParent] = sibling + m_child2[newParent] = leaf + m_parent[sibling] = newParent + m_parent[leaf] = newParent + } else { + // The sibling was the root. + m_child1[newParent] = sibling + m_child2[newParent] = leaf + m_parent[sibling] = newParent + m_parent[leaf] = newParent + m_root = newParent + } + + // Walk back up the tree fixing heights and AABBs + index = m_parent[leaf] + while (index != NULL_NODE) { + index = balance(index) + + val child1 = m_child1[index] + val child2 = m_child2[index] + + assert(child1 != NULL_NODE) + assert(child2 != NULL_NODE) + + m_height[index] = 1 + MathUtils.max(m_height[child1], m_height[child2]) + m_aabb[index].combine(m_aabb[child1], m_aabb[child2]) + + index = m_parent[index] + } + // validate(); + } + + private fun removeLeaf(leaf: Int) { + if (leaf == m_root) { + m_root = NULL_NODE + return + } + + val parent = m_parent[leaf] + val grandParent = m_parent[parent] + val parentChild1 = m_child1[parent] + val parentChild2 = m_child2[parent] + val sibling: Int + if (parentChild1 == leaf) { + sibling = parentChild2 + } else { + sibling = parentChild1 + } + + if (grandParent != NULL_NODE) { + // Destroy parent and connect sibling to grandParent. + if (m_child1[grandParent] == parent) { + m_child1[grandParent] = sibling + } else { + m_child2[grandParent] = sibling + } + m_parent[sibling] = grandParent + freeNode(parent) + + // Adjust ancestor bounds. + var index = grandParent + while (index != NULL_NODE) { + index = balance(index) + + val child1 = m_child1[index] + val child2 = m_child2[index] + + m_aabb[index].combine(m_aabb[child1], m_aabb[child2]) + m_height[index] = 1 + MathUtils.max(m_height[child1], m_height[child2]) + + index = m_parent[index] + } + } else { + m_root = sibling + m_parent[sibling] = NULL_NODE + freeNode(parent) + } + + // validate(); + } + + // Perform a left or right rotation if node A is imbalanced. + // Returns the new root index. + private fun balance(iA: Int): Int { + assert(iA != NULL_NODE) + + val A = iA + if (m_child1[A] == NULL_NODE || m_height[A] < 2) { + return iA + } + + val iB = m_child1[A] + val iC = m_child2[A] + assert(0 <= iB && iB < m_nodeCapacity) + assert(0 <= iC && iC < m_nodeCapacity) + + val B = iB + val C = iC + + val balance = m_height[C] - m_height[B] + + // Rotate C up + if (balance > 1) { + val iF = m_child1[C] + val iG = m_child2[C] + val F = iF + val G = iG + // assert (F != null); + // assert (G != null); + assert(0 <= iF && iF < m_nodeCapacity) + assert(0 <= iG && iG < m_nodeCapacity) + + // Swap A and C + m_child1[C] = iA + m_parent[C] = m_parent[A] + val cParent = m_parent[C] + m_parent[A] = iC + + // A's old parent should point to C + if (cParent != NULL_NODE) { + if (m_child1[cParent] == iA) { + m_child1[cParent] = iC + } else { + assert(m_child2[cParent] == iA) + m_child2[cParent] = iC + } + } else { + m_root = iC + } + + // Rotate + if (m_height[F] > m_height[G]) { + m_child2[C] = iF + m_child2[A] = iG + m_parent[G] = iA + m_aabb[A].combine(m_aabb[B], m_aabb[G]) + m_aabb[C].combine(m_aabb[A], m_aabb[F]) + + m_height[A] = 1 + MathUtils.max(m_height[B], m_height[G]) + m_height[C] = 1 + MathUtils.max(m_height[A], m_height[F]) + } else { + m_child2[C] = iG + m_child2[A] = iF + m_parent[F] = iA + m_aabb[A].combine(m_aabb[B], m_aabb[F]) + m_aabb[C].combine(m_aabb[A], m_aabb[G]) + + m_height[A] = 1 + MathUtils.max(m_height[B], m_height[F]) + m_height[C] = 1 + MathUtils.max(m_height[A], m_height[G]) + } + + return iC + } + + // Rotate B up + if (balance < -1) { + val iD = m_child1[B] + val iE = m_child2[B] + val D = iD + val E = iE + assert(0 <= iD && iD < m_nodeCapacity) + assert(0 <= iE && iE < m_nodeCapacity) + + // Swap A and B + m_child1[B] = iA + m_parent[B] = m_parent[A] + val Bparent = m_parent[B] + m_parent[A] = iB + + // A's old parent should point to B + if (Bparent != NULL_NODE) { + if (m_child1[Bparent] == iA) { + m_child1[Bparent] = iB + } else { + assert(m_child2[Bparent] == iA) + m_child2[Bparent] = iB + } + } else { + m_root = iB + } + + // Rotate + if (m_height[D] > m_height[E]) { + m_child2[B] = iD + m_child1[A] = iE + m_parent[E] = iA + m_aabb[A].combine(m_aabb[C], m_aabb[E]) + m_aabb[B].combine(m_aabb[A], m_aabb[D]) + + m_height[A] = 1 + MathUtils.max(m_height[C], m_height[E]) + m_height[B] = 1 + MathUtils.max(m_height[A], m_height[D]) + } else { + m_child2[B] = iE + m_child1[A] = iD + m_parent[D] = iA + m_aabb[A].combine(m_aabb[C], m_aabb[D]) + m_aabb[B].combine(m_aabb[A], m_aabb[E]) + + m_height[A] = 1 + MathUtils.max(m_height[C], m_height[D]) + m_height[B] = 1 + MathUtils.max(m_height[A], m_height[E]) + } + + return iB + } + + return iA + } + + private fun validateStructure(node: Int) { + if (node == NULL_NODE) { + return + } + + if (node == m_root) { + assert(m_parent[node] == NULL_NODE) + } + + val child1 = m_child1[node] + val child2 = m_child2[node] + + if (child1 == NULL_NODE) { + assert(child1 == NULL_NODE) + assert(child2 == NULL_NODE) + assert(m_height[node] == 0) + return + } + + assert(child1 != NULL_NODE && 0 <= child1 && child1 < m_nodeCapacity) + assert(child2 != NULL_NODE && 0 <= child2 && child2 < m_nodeCapacity) + + assert(m_parent[child1] == node) + assert(m_parent[child2] == node) + + validateStructure(child1) + validateStructure(child2) + } + + private fun validateMetrics(node: Int) { + if (node == NULL_NODE) { + return + } + + val child1 = m_child1[node] + val child2 = m_child2[node] + + if (child1 == NULL_NODE) { + assert(child1 == NULL_NODE) + assert(child2 == NULL_NODE) + assert(m_height[node] == 0) + return + } + + assert(child1 != NULL_NODE && 0 <= child1 && child1 < m_nodeCapacity) + assert(child2 != child1 && 0 <= child2 && child2 < m_nodeCapacity) + + val height1 = m_height[child1] + val height2 = m_height[child2] + val height: Int + height = 1 + MathUtils.max(height1, height2) + assert(m_height[node] == height) + + val aabb = AABB() + aabb.combine(m_aabb[child1], m_aabb[child2]) + + assert(aabb.lowerBound == m_aabb[node].lowerBound) + assert(aabb.upperBound == m_aabb[node].upperBound) + + validateMetrics(child1) + validateMetrics(child2) + } + + override fun drawTree(argDraw: DebugDraw) { + if (m_root == NULL_NODE) { + return + } + val height = computeHeight() + drawTree(argDraw, m_root, 0, height) + } + + fun drawTree(argDraw: DebugDraw, node: Int, spot: Int, height: Int) { + val a = m_aabb[node] + a.getVertices(drawVecs) + + color.set(1f, (height - spot) * 1f / height, (height - spot) * 1f / height) + argDraw.drawPolygon(drawVecs, 4, color) + + argDraw.viewportTranform!!.getWorldToScreen(a.upperBound, textVec) + argDraw.drawString(textVec.x, textVec.y, node.toString() + "-" + (spot + 1) + "/" + height, color) + + val c1 = m_child1[node] + val c2 = m_child2[node] + if (c1 != NULL_NODE) { + drawTree(argDraw, c1, spot + 1, height) + } + if (c2 != NULL_NODE) { + drawTree(argDraw, c2, spot + 1, height) + } + } + + companion object { + + val MAX_STACK_SIZE = 64 + + val NULL_NODE = -1 + + val INITIAL_BUFFER_LENGTH = 16 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeNode.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeNode.kt new file mode 100644 index 0000000..7881df8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/broadphase/DynamicTreeNode.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.broadphase + +import org.jbox2d.collision.AABB +import org.jbox2d.userdata.Box2dTypedUserData + +class DynamicTreeNode(internal val id: Int) : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + /** + * Enlarged AABB + */ + + val aabb = AABB() + + + var userData: Any? = Unit + + + internal var parent: DynamicTreeNode? = null + + internal var child1: DynamicTreeNode? = null + + internal var child2: DynamicTreeNode? = null + + internal var height: Int = 0 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ChainShape.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ChainShape.kt new file mode 100644 index 0000000..abd69ae --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ChainShape.kt @@ -0,0 +1,259 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert + +/** + * A chain shape is a free form sequence of line segments. The chain has two-sided collision, so you + * can use inside and outside collision. Therefore, you may use any winding order. Connectivity + * information is used to create smooth collisions. WARNING: The chain will not collide properly if + * there are self-intersections. + * + * @author Daniel + */ +class ChainShape : Shape(ShapeType.CHAIN) { + + var vertices: Array? = null + + var count: Int = 0 + + val prevVertex = Vec2() + val nextVertex = Vec2() + + var hasPrevVertex = false + var hasNextVertex = false + + private val pool0 = EdgeShape() + + init { + vertices = null + radius = Settings.polygonRadius + count = 0 + } + + fun clear() { + vertices = null + count = 0 + } + + override fun getChildCount(): Int { + return count - 1 + } + + /** + * Get a child edge. + */ + fun getChildEdge(edge: EdgeShape, index: Int) { + assert(0 <= index && index < count - 1) + edge.radius = radius + + val v0 = vertices!![index + 0] + val v1 = vertices!![index + 1] + edge.vertex1.x = v0.x + edge.vertex1.y = v0.y + edge.vertex2.x = v1.x + edge.vertex2.y = v1.y + + if (index > 0) { + val v = vertices!![index - 1] + edge.vertex0.x = v.x + edge.vertex0.y = v.y + edge.hasVertex0 = true + } else { + edge.vertex0.x = prevVertex.x + edge.vertex0.y = prevVertex.y + edge.hasVertex0 = hasPrevVertex + } + + if (index < count - 2) { + val v = vertices!![index + 2] + edge.vertex3.x = v.x + edge.vertex3.y = v.y + edge.hasVertex3 = true + } else { + edge.vertex3.x = nextVertex.x + edge.vertex3.y = nextVertex.y + edge.hasVertex3 = hasNextVertex + } + } + + override fun computeDistanceToOut(xf: Transform, p: Vec2, childIndex: Int, normalOut: Vec2): Float { + val edge = pool0 + getChildEdge(edge, childIndex) + return edge.computeDistanceToOut(xf, p, 0, normalOut) + } + + override fun testPoint(xf: Transform, p: Vec2): Boolean { + return false + } + + override fun raycast(output: RayCastOutput, input: RayCastInput, xf: Transform, childIndex: Int): Boolean { + assert(childIndex < count) + + val edgeShape = pool0 + + val i1 = childIndex + var i2 = childIndex + 1 + if (i2 == count) { + i2 = 0 + } + val v = vertices!![i1] + edgeShape.vertex1.x = v.x + edgeShape.vertex1.y = v.y + val v1 = vertices!![i2] + edgeShape.vertex2.x = v1.x + edgeShape.vertex2.y = v1.y + + return edgeShape.raycast(output, input, xf, 0) + } + + override fun computeAABB(aabb: AABB, xf: Transform, childIndex: Int) { + assert(childIndex < count) + val lower = aabb.lowerBound + val upper = aabb.upperBound + + val i1 = childIndex + var i2 = childIndex + 1 + if (i2 == count) { + i2 = 0 + } + + val vi1 = vertices!![i1] + val vi2 = vertices!![i2] + val xfq = xf.q + val xfp = xf.p + val v1x = xfq.c * vi1.x - xfq.s * vi1.y + xfp.x + val v1y = xfq.s * vi1.x + xfq.c * vi1.y + xfp.y + val v2x = xfq.c * vi2.x - xfq.s * vi2.y + xfp.x + val v2y = xfq.s * vi2.x + xfq.c * vi2.y + xfp.y + + lower.x = if (v1x < v2x) v1x else v2x + lower.y = if (v1y < v2y) v1y else v2y + upper.x = if (v1x > v2x) v1x else v2x + upper.y = if (v1y > v2y) v1y else v2y + } + + override fun computeMass(massData: MassData, density: Float) { + massData.mass = 0.0f + massData.center.setZero() + massData.I = 0.0f + } + + override fun clone(): Shape { + val clone = ChainShape() + clone.createChain(vertices, count) + clone.prevVertex.set(prevVertex) + clone.nextVertex.set(nextVertex) + clone.hasPrevVertex = hasPrevVertex + clone.hasNextVertex = hasNextVertex + return clone + } + + /** + * Create a loop. This automatically adjusts connectivity. + * + * @param vertices an array of vertices, these are copied + * @param count the vertex count + */ + fun createLoop(vertices: Array, count: Int) { + assert(this.vertices == null && this.count == 0) + assert(count >= 3) + this.count = count + 1 + this.vertices = Array(this.count) { Vec2.dummy } + for (i in 1 until count) { + val v1 = vertices[i - 1] + val v2 = vertices[i] + // If the code crashes here, it means your vertices are too close together. + if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) { + throw RuntimeException("Vertices of chain shape are too close together") + } + } + for (i in 0 until count) { + this.vertices!![i] = Vec2(vertices[i]) + } + this.vertices!![count] = Vec2(this.vertices!![0]) + prevVertex.set(this.vertices!![this.count - 2]) + nextVertex.set(this.vertices!![1]) + hasPrevVertex = true + hasNextVertex = true + } + + /** + * Create a chain with isolated end vertices. + * + * @param vertices an array of vertices, these are copied + * @param count the vertex count + */ + fun createChain(vertices: Array?, count: Int) { + assert(this.vertices == null && this.count == 0) + assert(count >= 2) + this.count = count + this.vertices = Array(this.count) { Vec2.dummy } + for (i in 1 until this.count) { + val v1 = vertices!![i - 1] + val v2 = vertices[i] + // If the code crashes here, it means your vertices are too close together. + if (MathUtils.distanceSquared(v1, v2) < Settings.linearSlop * Settings.linearSlop) { + throw RuntimeException("Vertices of chain shape are too close together") + } + } + for (i in 0 until this.count) { + this.vertices!![i] = Vec2(vertices!![i]) + } + hasPrevVertex = false + hasNextVertex = false + + prevVertex.setZero() + nextVertex.setZero() + } + + /** + * Establish connectivity to a vertex that precedes the first vertex. Don't call this for loops. + * + * @param prevVertex + */ + fun setPrevVertex(prevVertex: Vec2) { + this.prevVertex.set(prevVertex) + hasPrevVertex = true + } + + /** + * Establish connectivity to a vertex that follows the last vertex. Don't call this for loops. + * + * @param nextVertex + */ + fun setNextVertex(nextVertex: Vec2) { + this.nextVertex.set(nextVertex) + hasNextVertex = true + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/CircleShape.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/CircleShape.kt new file mode 100644 index 0000000..36e8a6c --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/CircleShape.kt @@ -0,0 +1,191 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert + +/** + * A circle shape. + */ +class CircleShape() : Shape(ShapeType.CIRCLE) { + + val p: Vec2 = Vec2() + + /** + * Get the vertex count. + * + * @return + */ + val vertexCount: Int get() = 1 + + constructor(radius: Number) : this() { + this.radius = radius.toFloat() + } + + override fun clone(): Shape { + val shape = CircleShape() + shape.p.x = p.x + shape.p.y = p.y + shape.radius = radius + return shape + } + + override fun getChildCount(): Int = 1 + + /** + * Get the supporting vertex index in the given direction. + * + * @param d + * @return + */ + fun getSupport(d: Vec2): Int = 0 + + /** + * Get the supporting vertex in the given direction. + * + * @param d + * @return + */ + fun getSupportVertex(d: Vec2): Vec2 = p + + /** + * Get a vertex by index. + * + * @param index + * @return + */ + fun getVertex(index: Int): Vec2 { + assert(index == 0) + return p + } + + override fun testPoint(transform: Transform, p: Vec2): Boolean { + // Rot.mulToOutUnsafe(transform.q, m_p, center); + // center.addLocal(transform.p); + // + // final Vec2 d = center.subLocal(p).negateLocal(); + // return Vec2.dot(d, d) <= m_radius * m_radius; + val q = transform.q + val tp = transform.p + val centerx = -(q.c * this.p.x - q.s * this.p.y + tp.x - p.x) + val centery = -(q.s * this.p.x + q.c * this.p.y + tp.y - p.y) + + return centerx * centerx + centery * centery <= radius * radius + } + + override fun computeDistanceToOut(xf: Transform, p: Vec2, childIndex: Int, normalOut: Vec2): Float { + val xfq = xf.q + val centerx = xfq.c * this.p.x - xfq.s * this.p.y + xf.p.x + val centery = xfq.s * this.p.x + xfq.c * this.p.y + xf.p.y + val dx = p.x - centerx + val dy = p.y - centery + val d1 = MathUtils.sqrt(dx * dx + dy * dy) + normalOut.x = dx * 1 / d1 + normalOut.y = dy * 1 / d1 + return d1 - radius + } + + // Collision Detection in Interactive 3D Environments by Gino van den Bergen + // From Section 3.1.2 + // x = s + a * r + // norm(x) = radius + override fun raycast( + output: RayCastOutput, input: RayCastInput, + transform: Transform, childIndex: Int + ): Boolean { + + val inputp1 = input.p1 + val inputp2 = input.p2 + val tq = transform.q + val tp = transform.p + + // Rot.mulToOutUnsafe(transform.q, m_p, position); + // position.addLocal(transform.p); + val positionx = tq.c * p.x - tq.s * p.y + tp.x + val positiony = tq.s * p.x + tq.c * p.y + tp.y + + val sx = inputp1.x - positionx + val sy = inputp1.y - positiony + // final float b = Vec2.dot(s, s) - m_radius * m_radius; + val b = sx * sx + sy * sy - radius * radius + + // Solve quadratic equation. + val rx = inputp2.x - inputp1.x + val ry = inputp2.y - inputp1.y + // final float c = Vec2.dot(s, r); + // final float rr = Vec2.dot(r, r); + val c = sx * rx + sy * ry + val rr = rx * rx + ry * ry + val sigma = c * c - rr * b + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < Settings.EPSILON) { + return false + } + + // Find the point of intersection of the line with the circle. + var a = -(c + MathUtils.sqrt(sigma)) + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.maxFraction * rr) { + a /= rr + output.fraction = a + output.normal.x = rx * a + sx + output.normal.y = ry * a + sy + output.normal.normalize() + return true + } + + return false + } + + override fun computeAABB(aabb: AABB, transform: Transform, childIndex: Int) { + val tq = transform.q + val tp = transform.p + val px = tq.c * p.x - tq.s * p.y + tp.x + val py = tq.s * p.x + tq.c * p.y + tp.y + + aabb.lowerBound.x = px - radius + aabb.lowerBound.y = py - radius + aabb.upperBound.x = px + radius + aabb.upperBound.y = py + radius + } + + override fun computeMass(massData: MassData, density: Float) { + massData.mass = density * Settings.PI * radius * radius + massData.center.x = p.x + massData.center.y = p.y + + // inertia about the local origin + // massData.I = massData.mass * (0.5f * m_radius * m_radius + Vec2.dot(m_p, m_p)); + massData.I = massData.mass * (0.5f * radius * radius + (p.x * p.x + p.y * p.y)) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/EdgeShape.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/EdgeShape.kt new file mode 100644 index 0000000..9079e05 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/EdgeShape.kt @@ -0,0 +1,257 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 + +/** + * A line segment (edge) shape. These can be connected in chains or loops to other edge shapes. The + * connectivity information is used to ensure correct contact normals. + * + * @author Daniel + */ +class EdgeShape() : Shape(ShapeType.EDGE) { + + /** + * edge vertex 1 + */ + + val vertex1 = Vec2() + /** + * edge vertex 2 + */ + + val vertex2 = Vec2() + + /** + * optional adjacent vertex 1. Used for smooth collision + */ + + val vertex0 = Vec2() + /** + * optional adjacent vertex 2. Used for smooth collision + */ + + val vertex3 = Vec2() + + var hasVertex0 = false + + var hasVertex3 = false + + // for pooling + private val normal = Vec2() + + constructor(x1: Number, y1: Number, x2: Number, y2: Number) : this() { + set(Vec2(x1.toFloat(), y1.toFloat()), Vec2(x2.toFloat(), y2.toFloat())) + } + + init { + radius = Settings.polygonRadius + } + + override fun getChildCount(): Int { + return 1 + } + + fun set(v1: Vec2, v2: Vec2) { + vertex1.set(v1) + vertex2.set(v2) + hasVertex3 = false + hasVertex0 = hasVertex3 + } + + override fun testPoint(xf: Transform, p: Vec2): Boolean { + return false + } + + override fun computeDistanceToOut(xf: Transform, p: Vec2, childIndex: Int, normalOut: Vec2): Float { + val xfqc = xf.q.c + val xfqs = xf.q.s + val xfpx = xf.p.x + val xfpy = xf.p.y + val v1x = xfqc * vertex1.x - xfqs * vertex1.y + xfpx + val v1y = xfqs * vertex1.x + xfqc * vertex1.y + xfpy + val v2x = xfqc * vertex2.x - xfqs * vertex2.y + xfpx + val v2y = xfqs * vertex2.x + xfqc * vertex2.y + xfpy + + var dx = p.x - v1x + var dy = p.y - v1y + val sx = v2x - v1x + val sy = v2y - v1y + val ds = dx * sx + dy * sy + if (ds > 0) { + val s2 = sx * sx + sy * sy + if (ds > s2) { + dx = p.x - v2x + dy = p.y - v2y + } else { + dx -= ds / s2 * sx + dy -= ds / s2 * sy + } + } + + val d1 = MathUtils.sqrt(dx * dx + dy * dy) + if (d1 > 0) { + normalOut.x = 1 / d1 * dx + normalOut.y = 1 / d1 * dy + } else { + normalOut.x = 0f + normalOut.y = 0f + } + return d1 + } + + // p = p1 + t * d + // v = v1 + s * e + // p1 + t * d = v1 + s * e + // s * e - t * d = p1 - v1 + override fun raycast(output: RayCastOutput, input: RayCastInput, xf: Transform, childIndex: Int): Boolean { + + var tempx: Float + var tempy: Float + val v1 = vertex1 + val v2 = vertex2 + val xfq = xf.q + val xfp = xf.p + + // Put the ray into the edge's frame of reference. + // b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + // b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + tempx = input.p1.x - xfp.x + tempy = input.p1.y - xfp.y + val p1x = xfq.c * tempx + xfq.s * tempy + val p1y = -xfq.s * tempx + xfq.c * tempy + + tempx = input.p2.x - xfp.x + tempy = input.p2.y - xfp.y + val p2x = xfq.c * tempx + xfq.s * tempy + val p2y = -xfq.s * tempx + xfq.c * tempy + + val dx = p2x - p1x + val dy = p2y - p1y + + // final Vec2 normal = pool2.set(v2).subLocal(v1); + // normal.set(normal.y, -normal.x); + normal.x = v2.y - v1.y + normal.y = v1.x - v2.x + normal.normalize() + val normalx = normal.x + val normaly = normal.y + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + tempx = v1.x - p1x + tempy = v1.y - p1y + val numerator = normalx * tempx + normaly * tempy + val denominator = normalx * dx + normaly * dy + + if (denominator == 0.0f) { + return false + } + + val t = numerator / denominator + if (t < 0.0f || 1.0f < t) { + return false + } + + // Vec2 q = p1 + t * d; + val qx = p1x + t * dx + val qy = p1y + t * dy + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + // Vec2 r = v2 - v1; + val rx = v2.x - v1.x + val ry = v2.y - v1.y + val rr = rx * rx + ry * ry + if (rr == 0.0f) { + return false + } + tempx = qx - v1.x + tempy = qy - v1.y + // float s = Vec2.dot(pool5, r) / rr; + val s = (tempx * rx + tempy * ry) / rr + if (s < 0.0f || 1.0f < s) { + return false + } + + output.fraction = t + if (numerator > 0.0f) { + // output.normal = -b2Mul(xf.q, normal); + output.normal.x = -xfq.c * normal.x + xfq.s * normal.y + output.normal.y = -xfq.s * normal.x - xfq.c * normal.y + } else { + // output->normal = b2Mul(xf.q, normal); + output.normal.x = xfq.c * normal.x - xfq.s * normal.y + output.normal.y = xfq.s * normal.x + xfq.c * normal.y + } + return true + } + + override fun computeAABB(aabb: AABB, xf: Transform, childIndex: Int) { + val lowerBound = aabb.lowerBound + val upperBound = aabb.upperBound + val xfq = xf.q + + val v1x = xfq.c * vertex1.x - xfq.s * vertex1.y + xf.p.x + val v1y = xfq.s * vertex1.x + xfq.c * vertex1.y + xf.p.y + val v2x = xfq.c * vertex2.x - xfq.s * vertex2.y + xf.p.x + val v2y = xfq.s * vertex2.x + xfq.c * vertex2.y + xf.p.y + + lowerBound.x = if (v1x < v2x) v1x else v2x + lowerBound.y = if (v1y < v2y) v1y else v2y + upperBound.x = if (v1x > v2x) v1x else v2x + upperBound.y = if (v1y > v2y) v1y else v2y + + lowerBound.x -= radius + lowerBound.y -= radius + upperBound.x += radius + upperBound.y += radius + } + + override fun computeMass(massData: MassData, density: Float) { + massData.mass = 0.0f + massData.center.set(vertex1).addLocal(vertex2).mulLocal(0.5f) + massData.I = 0.0f + } + + override fun clone(): Shape { + val edge = EdgeShape() + edge.radius = this.radius + edge.hasVertex0 = this.hasVertex0 + edge.hasVertex3 = this.hasVertex3 + edge.vertex0.set(this.vertex0) + edge.vertex1.set(this.vertex1) + edge.vertex2.set(this.vertex2) + edge.vertex3.set(this.vertex3) + return edge + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/MassData.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/MassData.kt new file mode 100644 index 0000000..f5a8d78 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/MassData.kt @@ -0,0 +1,95 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.collision.shapes + +import org.jbox2d.common.Vec2 + +// Updated to rev 100 + +/** This holds the mass data computed for a shape. */ +class MassData { + /** The mass of the shape, usually in kilograms. */ + var mass: Float = 0.toFloat() + /** The position of the shape's centroid relative to the shape's origin. */ + + val center: Vec2 + /** The rotational inertia of the shape about the local origin. */ + + var I: Float = 0.toFloat() + + /** + * Blank mass data + */ + constructor() { + I = 0f + mass = I + center = Vec2() + } + + /** + * Copies from the given mass data + * + * @param md + * mass data to copy from + */ + constructor(md: MassData) { + mass = md.mass + I = md.I + center = md.center.clone() + } + + fun set(md: MassData) { + mass = md.mass + I = md.I + center.set(md.center) + } + + /** Return a copy of this object. */ + fun clone(): MassData { + return MassData(this) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/PolygonShape.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/PolygonShape.kt new file mode 100644 index 0000000..4dbb36d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/PolygonShape.kt @@ -0,0 +1,671 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + +import korlibs.math.geom.Angle +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert +import org.jbox2d.pooling.arrays.IntArrayPool +import org.jbox2d.pooling.arrays.Vec2ArrayPool + +/** + * A convex polygon shape. Polygons have a maximum number of vertices equal to _maxPolygonVertices. + * In most cases you should not need many vertices for a convex polygon. + */ +class PolygonShape : Shape(ShapeType.POLYGON) { + + /** + * Local position of the shape centroid in parent body frame. + */ + + val centroid = Vec2() + + /** + * The vertices of the shape. Note: use getVertexCount(), not m_vertices.length, to get number of + * active vertices. + */ + /** Get the vertices in local coordinates. */ + + val vertices: Array = Array(Settings.maxPolygonVertices) { Vec2() } + + /** + * The normals of the shape. Note: use getVertexCount(), not m_normals.length, to get number of + * active normals. + */ + /** Get the edge normal vectors. There is one for each vertex. */ + + val normals: Array = Array(Settings.maxPolygonVertices) { Vec2() } + + /** + * Number of active vertices in the shape. + */ + /** + * Get the vertex count. + * + * @return + */ + + var count: Int = 0 + + // pooling + private val pool1 = Vec2() + private val pool2 = Vec2() + private val pool3 = Vec2() + private val pool4 = Vec2() + private val poolt1 = Transform() + + init { + radius = Settings.polygonRadius + centroid.setZero() + } + + override fun clone(): Shape { + val shape = PolygonShape() + shape.centroid.set(this.centroid) + for (i in shape.normals.indices) { + shape.normals[i].set(normals[i]) + shape.vertices[i].set(vertices[i]) + } + shape.radius = this.radius + shape.count = this.count + return shape + } + + /** + * Create a convex hull from the given array of points. The count must be in the range [3, + * Settings.maxPolygonVertices]. + * + * @warning the points may be re-ordered, even if they form a convex polygon. + * @warning collinear points are removed. + */ + fun set(vertices: Array, count: Int) { + set(vertices, count, null, null) + } + + /** + * Create a convex hull from the given array of points. The count must be in the range [3, + * Settings.maxPolygonVertices]. This method takes an arraypool for pooling. + * + * @warning the points may be re-ordered, even if they form a convex polygon. + * @warning collinear points are removed. + */ + fun set(verts: Array, num: Int, vecPool: Vec2ArrayPool?, + intPool: IntArrayPool?) { + assert(3 <= num && num <= Settings.maxPolygonVertices) + if (num < 3) { + setAsBox(1.0f, 1.0f) + return + } + + var n = MathUtils.min(num, Settings.maxPolygonVertices) + + // Perform welding and copy vertices into local buffer. + val ps : Array + if (vecPool != null) + ps = vecPool[Settings.maxPolygonVertices] as Array + else + ps = arrayOfNulls(Settings.maxPolygonVertices) + var tempCount = 0 + for (i in 0 until n) { + val v = verts[i] + var unique = true + for (j in 0 until tempCount) { + if (MathUtils.distanceSquared(v, ps[j]!!) < 0.5f * Settings.linearSlop) { + unique = false + break + } + } + + if (unique) { + ps[tempCount++] = v + } + } + + n = tempCount + if (n < 3) { + // Polygon is degenerate. + assert(false) + setAsBox(1.0f, 1.0f) + return + } + + // Create the convex hull using the Gift wrapping algorithm + // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm + + // Find the right most point on the hull + var i0 = 0 + var x0 = ps[0]!!.x + for (i in 1 until n) { + val x = ps[i]!!.x + if (x > x0 || x == x0 && ps[i]!!.y < ps[i0]!!.y) { + i0 = i + x0 = x + } + } + + val hull = if (intPool != null) + intPool[Settings.maxPolygonVertices] + else + IntArray(Settings.maxPolygonVertices) + var m = 0 + var ih = i0 + + while (true) { + hull[m] = ih + + var ie = 0 + for (j in 1 until n) { + if (ie == ih) { + ie = j + continue + } + + val r = pool1.set(ps[ie]!!).subLocal(ps[hull[m]]!!) + val v = pool2.set(ps[j]!!).subLocal(ps[hull[m]]!!) + val c = Vec2.cross(r, v) + if (c < 0.0f) { + ie = j + } + + // Collinearity check + if (c == 0.0f && v.lengthSquared() > r.lengthSquared()) { + ie = j + } + } + + ++m + ih = ie + + if (ie == i0) { + break + } + } + + this.count = m + + // Copy vertices. + for (i in 0 until count) { + vertices[i].set(ps[hull[i]]!!) + } + + val edge = pool1 + + // Compute normals. Ensure the edges have non-zero length. + for (i in 0 until count) { + val i1 = i + val i2 = if (i + 1 < count) i + 1 else 0 + edge.set(vertices[i2]).subLocal(vertices[i1]) + + assert(edge.lengthSquared() > Settings.EPSILON * Settings.EPSILON) + Vec2.crossToOutUnsafe(edge, 1f, normals[i]) + normals[i].normalize() + } + + // Compute the polygon centroid. + computeCentroidToOut(vertices, count, centroid) + } + + /** + * Build vertices to represent an axis-aligned box. + * + * @param hx the half-width. + * @param hy the half-height. + */ + fun setAsBox(hx: Float, hy: Float) { + count = 4 + vertices[0].set(-hx, -hy) + vertices[1].set(hx, -hy) + vertices[2].set(hx, hy) + vertices[3].set(-hx, hy) + normals[0].set(0.0f, -1.0f) + normals[1].set(1.0f, 0.0f) + normals[2].set(0.0f, 1.0f) + normals[3].set(-1.0f, 0.0f) + centroid.setZero() + } + + /** + * Build vertices to represent an oriented box. + * + * @param hx the half-width. + * @param hy the half-height. + * @param center the center of the box in local coordinates. + * @param angleRadians the rotation of the box in local coordinates. + */ + fun setAsBoxRadians(hx: Float, hy: Float, center: Vec2, angleRadians: Float) { + count = 4 + vertices[0].set(-hx, -hy) + vertices[1].set(hx, -hy) + vertices[2].set(hx, hy) + vertices[3].set(-hx, hy) + normals[0].set(0.0f, -1.0f) + normals[1].set(1.0f, 0.0f) + normals[2].set(0.0f, 1.0f) + normals[3].set(-1.0f, 0.0f) + centroid.set(center) + + val xf = poolt1 + xf.p.set(center) + xf.q.setRadians(angleRadians) + + // Transform vertices and normals. + for (i in 0 until count) { + Transform.mulToOut(xf, vertices[i], vertices[i]) + Rot.mulToOut(xf.q, normals[i], normals[i]) + } + } + + fun setAsBoxDegrees(hx: Float, hy: Float, center: Vec2, angleDegrees: Float) = setAsBoxRadians(hx, hy, center, angleDegrees * MathUtils.DEG2RAD) + + fun setAsBox(hx: Float, hy: Float, center: Vec2, angle: Angle) = setAsBoxRadians(hx, hy, center, angle.radians.toFloat()) + + override fun getChildCount(): Int { + return 1 + } + + override fun testPoint(xf: Transform, p: Vec2): Boolean { + var tempx: Float + var tempy: Float + val xfq = xf.q + + tempx = p.x - xf.p.x + tempy = p.y - xf.p.y + val pLocalx = xfq.c * tempx + xfq.s * tempy + val pLocaly = -xfq.s * tempx + xfq.c * tempy + + if (m_debug) { + println("--testPoint debug--") + println("Vertices: ") + for (i in 0 until count) { + println(vertices[i]) + } + println("pLocal: $pLocalx, $pLocaly") + } + + for (i in 0 until count) { + val vertex = vertices[i] + val normal = normals[i] + tempx = pLocalx - vertex.x + tempy = pLocaly - vertex.y + val dot = normal.x * tempx + normal.y * tempy + if (dot > 0.0f) { + return false + } + } + + return true + } + + override fun computeAABB(aabb: AABB, xf: Transform, childIndex: Int) { + val lower = aabb.lowerBound + val upper = aabb.upperBound + val v1 = vertices[0] + val xfqc = xf.q.c + val xfqs = xf.q.s + val xfpx = xf.p.x + val xfpy = xf.p.y + lower.x = xfqc * v1.x - xfqs * v1.y + xfpx + lower.y = xfqs * v1.x + xfqc * v1.y + xfpy + upper.x = lower.x + upper.y = lower.y + + for (i in 1 until count) { + val v2 = vertices[i] + // Vec2 v = Mul(xf, m_vertices[i]); + val vx = xfqc * v2.x - xfqs * v2.y + xfpx + val vy = xfqs * v2.x + xfqc * v2.y + xfpy + lower.x = if (lower.x < vx) lower.x else vx + lower.y = if (lower.y < vy) lower.y else vy + upper.x = if (upper.x > vx) upper.x else vx + upper.y = if (upper.y > vy) upper.y else vy + } + + lower.x -= radius + lower.y -= radius + upper.x += radius + upper.y += radius + } + + /** + * Get a vertex by index. + * + * @param index + * @return + */ + fun getVertex(index: Int): Vec2 { + assert(0 <= index && index < count) + return vertices[index] + } + + override fun computeDistanceToOut(xf: Transform, p: Vec2, childIndex: Int, normalOut: Vec2): Float { + val xfqc = xf.q.c + val xfqs = xf.q.s + var tx = p.x - xf.p.x + var ty = p.y - xf.p.y + val pLocalx = xfqc * tx + xfqs * ty + val pLocaly = -xfqs * tx + xfqc * ty + + var maxDistance = -Float.MAX_VALUE + var normalForMaxDistanceX = pLocalx + var normalForMaxDistanceY = pLocaly + + for (i in 0 until count) { + val vertex = vertices[i] + val normal = normals[i] + tx = pLocalx - vertex.x + ty = pLocaly - vertex.y + val dot = normal.x * tx + normal.y * ty + if (dot > maxDistance) { + maxDistance = dot + normalForMaxDistanceX = normal.x + normalForMaxDistanceY = normal.y + } + } + + val distance: Float + if (maxDistance > 0) { + var minDistanceX = normalForMaxDistanceX + var minDistanceY = normalForMaxDistanceY + var minDistance2 = maxDistance * maxDistance + for (i in 0 until count) { + val vertex = vertices[i] + val distanceVecX = pLocalx - vertex.x + val distanceVecY = pLocaly - vertex.y + val distance2 = distanceVecX * distanceVecX + distanceVecY * distanceVecY + if (minDistance2 > distance2) { + minDistanceX = distanceVecX + minDistanceY = distanceVecY + minDistance2 = distance2 + } + } + distance = MathUtils.sqrt(minDistance2) + normalOut.x = xfqc * minDistanceX - xfqs * minDistanceY + normalOut.y = xfqs * minDistanceX + xfqc * minDistanceY + normalOut.normalize() + } else { + distance = maxDistance + normalOut.x = xfqc * normalForMaxDistanceX - xfqs * normalForMaxDistanceY + normalOut.y = xfqs * normalForMaxDistanceX + xfqc * normalForMaxDistanceY + } + + return distance + } + + override fun raycast(output: RayCastOutput, input: RayCastInput, xf: Transform, + childIndex: Int): Boolean { + val xfqc = xf.q.c + val xfqs = xf.q.s + val xfp = xf.p + var tempx: Float + var tempy: Float + // b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + // b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + tempx = input.p1.x - xfp.x + tempy = input.p1.y - xfp.y + val p1x = xfqc * tempx + xfqs * tempy + val p1y = -xfqs * tempx + xfqc * tempy + + tempx = input.p2.x - xfp.x + tempy = input.p2.y - xfp.y + val p2x = xfqc * tempx + xfqs * tempy + val p2y = -xfqs * tempx + xfqc * tempy + + val dx = p2x - p1x + val dy = p2y - p1y + + var lower = 0f + var upper = input.maxFraction + + var index = -1 + + for (i in 0 until count) { + val normal = normals[i] + val vertex = vertices[i] + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + val tempxn = vertex.x - p1x + val tempyn = vertex.y - p1y + val numerator = normal.x * tempxn + normal.y * tempyn + val denominator = normal.x * dx + normal.y * dy + + if (denominator == 0.0f) { + if (numerator < 0.0f) { + return false + } + } else { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > + // numerator. + if (denominator < 0.0f && numerator < lower * denominator) { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator + index = i + } else if (denominator > 0.0f && numerator < upper * denominator) { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator + } + } + + if (upper < lower) { + return false + } + } + + assert(0.0f <= lower && lower <= input.maxFraction) + + if (index >= 0) { + output.fraction = lower + // normal = Mul(xf.R, m_normals[index]); + val normal = normals[index] + val out = output.normal + out.x = xfqc * normal.x - xfqs * normal.y + out.y = xfqs * normal.x + xfqc * normal.y + return true + } + return false + } + + fun computeCentroidToOut(vs: Array, count: Int, out: Vec2) { + assert(count >= 3) + + out.set(0.0f, 0.0f) + var area = 0.0f + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + val pRef = pool1 + pRef.setZero() + + val e1 = pool2 + val e2 = pool3 + + val inv3 = 1.0f / 3.0f + + for (i in 0 until count) { + // Triangle vertices. + val p1 = pRef + val p2 = vs[i] + val p3 = if (i + 1 < count) vs[i + 1] else vs[0] + + e1.set(p2).subLocal(p1) + e2.set(p3).subLocal(p1) + + val D = Vec2.cross(e1, e2) + + val triangleArea = 0.5f * D + area += triangleArea + + // Area weighted centroid + e1.set(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3) + out.addLocal(e1) + } + + // Centroid + assert(area > Settings.EPSILON) + out.mulLocal(1.0f / area) + } + + override fun computeMass(massData: MassData, density: Float) { + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + assert(count >= 3) + + val center = pool1 + center.setZero() + var area = 0.0f + var I = 0.0f + + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + val s = pool2 + s.setZero() + // This code would put the reference point inside the polygon. + for (i in 0 until count) { + s.addLocal(vertices[i]) + } + s.mulLocal(1.0f / count) + + val k_inv3 = 1.0f / 3.0f + + val e1 = pool3 + val e2 = pool4 + + for (i in 0 until count) { + // Triangle vertices. + e1.set(vertices[i]).subLocal(s) + e2.set(s).negateLocal().addLocal(if (i + 1 < count) vertices[i + 1] else vertices[0]) + + val D = Vec2.cross(e1, e2) + + val triangleArea = 0.5f * D + area += triangleArea + + // Area weighted centroid + center.x += triangleArea * k_inv3 * (e1.x + e2.x) + center.y += triangleArea * k_inv3 * (e1.y + e2.y) + + val ex1 = e1.x + val ey1 = e1.y + val ex2 = e2.x + val ey2 = e2.y + + val intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2 + val inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2 + + I += 0.25f * k_inv3 * D * (intx2 + inty2) + } + + // Total mass + massData.mass = density * area + + // Center of mass + assert(area > Settings.EPSILON) + center.mulLocal(1.0f / area) + massData.center.set(center).addLocal(s) + + // Inertia tensor relative to the local origin (point s) + massData.I = I * density + + // Shift to center of mass then to original body origin. + massData.I += massData.mass * Vec2.dot(massData.center, massData.center) + } + + /** + * Validate convexity. This is a very time consuming operation. + * + * @return + */ + fun validate(): Boolean { + for (i in 0 until count) { + val i1 = i + val i2 = if (i < count - 1) i1 + 1 else 0 + val p = vertices[i1] + val e = pool1.set(vertices[i2]).subLocal(p) + + for (j in 0 until count) { + if (j == i1 || j == i2) { + continue + } + + val v = pool2.set(vertices[j]).subLocal(p) + val c = Vec2.cross(e, v) + if (c < 0.0f) { + return false + } + } + } + + return true + } + + /** Get the centroid and apply the supplied transform. */ + fun centroid(xf: Transform): Vec2 { + return Transform.mul(xf, centroid) + } + + /** Get the centroid and apply the supplied transform. */ + fun centroidToOut(xf: Transform, out: Vec2): Vec2 { + Transform.mulToOutUnsafe(xf, centroid, out) + return out + } + + companion object { + /** Dump lots of debug information. */ + private val m_debug = false + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/Shape.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/Shape.kt new file mode 100644 index 0000000..8cc12ff --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/Shape.kt @@ -0,0 +1,110 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 + +/** + * A shape is used for collision detection. You can create a shape however you like. Shapes used for + * simulation in World are created automatically when a Fixture is created. Shapes may encapsulate a + * one or more child shapes. + */ +abstract class Shape(private val m_type: ShapeType) { + /** + * The radius of the underlying shape. This can refer to different things depending on the shape + * implementation + * + * @return + */ + var radius: Float = 0f + + /** + * Get the type of this shape. You can use this to down cast to the concrete shape. + * + * @return the shape type. + */ + fun getType(): ShapeType = m_type + + /** + * Get the number of child primitives + * + * @return + */ + abstract fun getChildCount(): Int + + /** + * Test a point for containment in this shape. This only works for convex shapes. + * + * @param xf the shape world transform. + * @param p a point in world coordinates. + */ + abstract fun testPoint(xf: Transform, p: Vec2): Boolean + + /** + * Cast a ray against a child shape. + * + * @param argOutput the ray-cast results. + * @param argInput the ray-cast input parameters. + * @param argTransform the transform to be applied to the shape. + * @param argChildIndex the child shape index + * @return if hit + */ + abstract fun raycast(output: RayCastOutput, input: RayCastInput, transform: Transform, + childIndex: Int): Boolean + + + /** + * Given a transform, compute the associated axis aligned bounding box for a child shape. + * + * @param argAabb returns the axis aligned box. + * @param argXf the world transform of the shape. + */ + abstract fun computeAABB(aabb: AABB, xf: Transform, childIndex: Int) + + /** + * Compute the mass properties of this shape using its dimensions and density. The inertia tensor + * is computed about the local origin. + * + * @param massData returns the mass data for this shape. + * @param density the density in kilograms per meter squared. + */ + abstract fun computeMass(massData: MassData, density: Float) + + /** + * Compute the distance from the current shape to the specified point. This only works for convex + * shapes. + * + * @param xf the shape world transform. + * @param p a point in world coordinates. + * @param normalOut returns the direction in which the distance increases. + * @return distance returns the distance from the current shape. + */ + abstract fun computeDistanceToOut(xf: Transform, p: Vec2, childIndex: Int, normalOut: Vec2): Float + + abstract fun clone(): Shape +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ShapeType.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ShapeType.kt new file mode 100644 index 0000000..c6dd89d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/collision/shapes/ShapeType.kt @@ -0,0 +1,32 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.collision.shapes + +/** + * Types of shapes + * @author Daniel + */ +enum class ShapeType { + CIRCLE, EDGE, POLYGON, CHAIN +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/BufferUtils.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/BufferUtils.kt new file mode 100644 index 0000000..da5cfa0 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/BufferUtils.kt @@ -0,0 +1,152 @@ +package org.jbox2d.common + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + +object BufferUtils { + /** Reallocate a buffer. */ + + fun reallocateBuffer( + klass: () -> T, oldBuffer: Array?, oldCapacity: Int, + newCapacity: Int + ): Array { + assert(newCapacity > oldCapacity) + return Array(newCapacity) { if (oldBuffer != null && it < oldCapacity) oldBuffer[it] else klass() } as Array + } + + /** Reallocate a buffer. */ + + fun reallocateBuffer(oldBuffer: IntArray?, oldCapacity: Int, newCapacity: Int): IntArray { + assert(newCapacity > oldCapacity) + val newBuffer = IntArray(newCapacity) + if (oldBuffer != null) { + arraycopy(oldBuffer, 0, newBuffer, 0, oldCapacity) + } + return newBuffer + } + + /** Reallocate a buffer. */ + + fun reallocateBuffer(oldBuffer: FloatArray?, oldCapacity: Int, newCapacity: Int): FloatArray { + assert(newCapacity > oldCapacity) + val newBuffer = FloatArray(newCapacity) + if (oldBuffer != null) { + arraycopy(oldBuffer, 0, newBuffer, 0, oldCapacity) + } + return newBuffer + } + + /** + * Reallocate a buffer. A 'deferred' buffer is reallocated only if it is not NULL. If + * 'userSuppliedCapacity' is not zero, buffer is user supplied and must be kept. + */ + + fun reallocateBuffer( + klass: () -> T, buffer: Array?, userSuppliedCapacity: Int, + oldCapacity: Int, newCapacity: Int, deferred: Boolean + ): Array { + var buffer = buffer + assert(newCapacity > oldCapacity) + assert(userSuppliedCapacity == 0 || newCapacity <= userSuppliedCapacity) + if ((!deferred || buffer != null) && userSuppliedCapacity == 0) { + buffer = reallocateBuffer(klass, buffer, oldCapacity, newCapacity) + } + return buffer!! + } + + /** + * Reallocate an int buffer. A 'deferred' buffer is reallocated only if it is not NULL. If + * 'userSuppliedCapacity' is not zero, buffer is user supplied and must be kept. + */ + + fun reallocateBuffer( + buffer: IntArray?, userSuppliedCapacity: Int, oldCapacity: Int, + newCapacity: Int, deferred: Boolean + ): IntArray { + var buffer = buffer + assert(newCapacity > oldCapacity) + assert(userSuppliedCapacity == 0 || newCapacity <= userSuppliedCapacity) + if ((!deferred || buffer != null) && userSuppliedCapacity == 0) { + buffer = reallocateBuffer(buffer, oldCapacity, newCapacity) + } + return buffer!! + } + + /** + * Reallocate a float buffer. A 'deferred' buffer is reallocated only if it is not NULL. If + * 'userSuppliedCapacity' is not zero, buffer is user supplied and must be kept. + */ + + fun reallocateBuffer( + buffer: FloatArray?, userSuppliedCapacity: Int, oldCapacity: Int, + newCapacity: Int, deferred: Boolean + ): FloatArray { + var buffer = buffer + assert(newCapacity > oldCapacity) + assert(userSuppliedCapacity == 0 || newCapacity <= userSuppliedCapacity) + if ((!deferred || buffer != null) && userSuppliedCapacity == 0) { + buffer = reallocateBuffer(buffer, oldCapacity, newCapacity) + } + return buffer!! + } + + /** Rotate an array, see std::rotate */ + + fun rotate(ray: Array, first: Int, new_first: Int, last: Int) { + var first = first + var new_first = new_first + var next = new_first + while (next != first) { + val temp = ray[first] + ray[first] = ray[next] + ray[next] = temp + first++ + next++ + if (next == last) { + next = new_first + } else if (first == new_first) { + new_first = next + } + } + } + + /** Rotate an array, see std::rotate */ + + fun rotate(ray: IntArray, first: Int, new_first: Int, last: Int) { + var first = first + var new_first = new_first + var next = new_first + while (next != first) { + val temp = ray[first] + ray[first] = ray[next] + ray[next] = temp + first++ + next++ + if (next == last) { + next = new_first + } else if (first == new_first) { + new_first = next + } + } + } + + /** Rotate an array, see std::rotate */ + + fun rotate(ray: FloatArray, first: Int, new_first: Int, last: Int) { + var first = first + var new_first = new_first + var next = new_first + while (next != first) { + val temp = ray[first] + ray[first] = ray[next] + ray[next] = temp + first++ + next++ + if (next == last) { + next = new_first + } else if (first == new_first) { + new_first = next + } + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Color3f.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Color3f.kt new file mode 100644 index 0000000..a84e814 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Color3f.kt @@ -0,0 +1,106 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.common + +// updated to rev 100 +/** + * Similar to javax.vecmath.Color3f holder + * @author ewjordan + */ +class Color3f { + + + var x: Float = 0.toFloat() + + var y: Float = 0.toFloat() + + var z: Float = 0.toFloat() + + + constructor() { + z = 0f + y = z + x = y + } + + constructor(r: Float, g: Float, b: Float) { + x = r + y = g + z = b + } + + fun set(r: Float, g: Float, b: Float) { + x = r + y = g + z = b + } + + fun set(argColor: Color3f) { + x = argColor.x + y = argColor.y + z = argColor.z + } + + // @TODO: Do not mutate those + companion object { + + //@ThreadLocal + val WHITE = Color3f(1f, 1f, 1f) + + //@ThreadLocal + val BLACK = Color3f(0f, 0f, 0f) + + //@ThreadLocal + val BLUE = Color3f(0f, 0f, 1f) + + //@ThreadLocal + val GREEN = Color3f(0f, 1f, 0f) + + //@ThreadLocal + val RED = Color3f(1f, 0f, 0f) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/IViewportTransform.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/IViewportTransform.kt new file mode 100644 index 0000000..a814064 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/IViewportTransform.kt @@ -0,0 +1,107 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +/** + * This is the viewport transform used from drawing. Use yFlip if you are drawing from the top-left + * corner. + * + * @author Daniel + */ +interface IViewportTransform { + + /** + * @return if the transform flips the y axis + */ + /** + * @param yFlip if we flip the y axis when transforming + */ + var isYFlip: Boolean + + /** + * This is the half-width and half-height. This should be the actual half-width and half-height, + * not anything transformed or scaled. Not a copy. + */ + /** + * This sets the half-width and half-height. This should be the actual half-width and half-height, + * not anything transformed or scaled. + */ + var extents: Vec2 + + /** + * center of the viewport. Not a copy. + */ + /** + * sets the center of the viewport. + */ + var center: Vec2 + + val mat22Representation: Mat22 + + /** + * This sets the half-width and half-height of the viewport. This should be the actual half-width + * and half-height, not anything transformed or scaled. + */ + fun setExtents(halfWidth: Float, halfHeight: Float) + + /** + * sets the center of the viewport. + */ + fun setCenter(x: Float, y: Float) + + /** + * Sets the transform's center to the given x and y coordinates, and using the given scale. + */ + fun setCamera(x: Float, y: Float, scale: Float) + + /** + * Transforms the given directional vector by the viewport transform (not positional) + */ + fun getWorldVectorToScreen(world: Vec2, screen: Vec2) + + + /** + * Transforms the given directional screen vector back to the world direction. + */ + fun getScreenVectorToWorld(screen: Vec2, world: Vec2) + + + /** + * takes the world coordinate (world) puts the corresponding screen coordinate in screen. It + * should be safe to give the same object as both parameters. + */ + fun getWorldToScreen(world: Vec2, screen: Vec2) + + + /** + * takes the screen coordinates (screen) and puts the corresponding world coordinates in world. It + * should be safe to give the same object as both parameters. + */ + fun getScreenToWorld(screen: Vec2, world: Vec2) + + /** + * Multiplies the viewport transform by the given Mat22 + */ + fun mulByTransform(transform: Mat22) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat22.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat22.kt new file mode 100644 index 0000000..e430f92 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat22.kt @@ -0,0 +1,629 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.internal.assert + +/** + * A 2-by-2 matrix. Stored in column-major order. + */ +class Mat22 { + + + val ex: Vec2 + + val ey: Vec2 + + /** + * Extract the angle in radians from this matrix (assumed to be a rotation matrix). + * + * @return + */ + val angleRadians: Float get() = MathUtils.atan2(ex.y, ex.x) + + /** + * Extract the angle in degrees from this matrix (assumed to be a rotation matrix). + * + * @return + */ + val angleDegrees: Float get() = angleRadians * MathUtils.RAD2DEG + + /** + * Extract the angle from this matrix (assumed to be a rotation matrix). + * + * @return + */ + val angle: Angle get() = angleRadians.radians + + /** Convert the matrix to printable format. */ + override fun toString(): String { + var s = "" + s += "[" + ex.x + "," + ey.x + "]\n" + s += "[" + ex.y + "," + ey.y + "]" + return s + } + + /** + * Construct zero matrix. Note: this is NOT an identity matrix! djm fixed double allocation + * problem + */ + constructor() { + ex = Vec2() + ey = Vec2() + } + + /** + * Create a matrix with given vectors as columns. + * + * @param c1 Column 1 of matrix + * @param c2 Column 2 of matrix + */ + constructor(c1: Vec2, c2: Vec2) { + ex = c1.clone() + ey = c2.clone() + } + + /** + * Create a matrix from four floats. + * + * @param exx + * @param col2x + * @param exy + * @param col2y + */ + constructor(exx: Float, col2x: Float, exy: Float, col2y: Float) { + ex = Vec2(exx, exy) + ey = Vec2(col2x, col2y) + } + + /** + * Set as a copy of another matrix. + * + * @param m Matrix to copy + */ + fun set(m: Mat22): Mat22 { + ex.x = m.ex.x + ex.y = m.ex.y + ey.x = m.ey.x + ey.y = m.ey.y + return this + } + + fun set(exx: Float, col2x: Float, exy: Float, col2y: Float): Mat22 { + ex.x = exx + ex.y = exy + ey.x = col2x + ey.y = col2y + return this + } + + /** + * Return a clone of this matrix. djm fixed double allocation + */ + // @Override // annotation omitted for GWT-compatibility + fun clone(): Mat22 { + return Mat22(ex!!, ey!!) + } + + /** + * Set as a matrix representing a rotation. + * + * @param angleRadians Rotation (in radians) that matrix represents. + */ + fun setRadians(angleRadians: Float) { + val c = MathUtils.cos(angleRadians) + val s = MathUtils.sin(angleRadians) + ex.x = c + ey.x = -s + ex.y = s + ey.y = c + } + + fun setDegrees(angleDegrees: Float) = setRadians(angleDegrees * MathUtils.DEG2RAD) + + fun set(angle: Angle) = setRadians(angle.radians.toFloat()) + + /** + * Set as the identity matrix. + */ + fun setIdentity() { + ex.x = 1.0f + ey.x = 0.0f + ex.y = 0.0f + ey.y = 1.0f + } + + /** + * Set as the zero matrix. + */ + fun setZero() { + ex.x = 0.0f + ey.x = 0.0f + ex.y = 0.0f + ey.y = 0.0f + } + + /** + * Set by column vectors. + * + * @param c1 Column 1 + * @param c2 Column 2 + */ + fun set(c1: Vec2, c2: Vec2) { + ex.x = c1.x + ey.x = c2.x + ex.y = c1.y + ey.y = c2.y + } + + /** Returns the inverted Mat22 - does NOT invert the matrix locally! */ + fun invert(): Mat22 { + val a = ex.x + val b = ey.x + val c = ex.y + val d = ey.y + val B = Mat22() + var det = a * d - b * c + if (det != 0f) { + det = 1.0f / det + } + B.ex.x = det * d + B.ey.x = -det * b + B.ex.y = -det * c + B.ey.y = det * a + return B + } + + fun invertLocal(): Mat22 { + val a = ex.x + val b = ey.x + val c = ex.y + val d = ey.y + var det = a * d - b * c + if (det != 0f) { + det = 1.0f / det + } + ex.x = det * d + ey.x = -det * b + ex.y = -det * c + ey.y = det * a + return this + } + + fun invertToOut(out: Mat22) { + val a = ex.x + val b = ey.x + val c = ex.y + val d = ey.y + var det = a * d - b * c + // b2Assert(det != 0.0f); + det = 1.0f / det + out.ex.x = det * d + out.ey.x = -det * b + out.ex.y = -det * c + out.ey.y = det * a + } + + + /** + * Return the matrix composed of the absolute values of all elements. djm: fixed double allocation + * + * @return Absolute value matrix + */ + fun abs(): Mat22 { + return Mat22(MathUtils.abs(ex.x), MathUtils.abs(ey.x), MathUtils.abs(ex.y), + MathUtils.abs(ey.y)) + } + + /* djm: added */ + fun absLocal() { + ex.absLocal() + ey.absLocal() + } + + /** + * Multiply a vector by this matrix. + * + * @param v Vector to multiply by matrix. + * @return Resulting vector + */ + fun mul(v: Vec2): Vec2 { + return Vec2(ex.x * v.x + ey.x * v.y, ex.y * v.x + ey.y * v.y) + } + + fun mulToOut(v: Vec2, out: Vec2) { + val tempy = ex.y * v.x + ey.y * v.y + out.x = ex.x * v.x + ey.x * v.y + out.y = tempy + } + + fun mulToOutUnsafe(v: Vec2, out: Vec2) { + assert(v !== out) + out.x = ex.x * v.x + ey.x * v.y + out.y = ex.y * v.x + ey.y * v.y + } + + + /** + * Multiply another matrix by this one (this one on left). djm optimized + * + * @param R + * @return + */ + fun mul(R: Mat22): Mat22 { + /* + * Mat22 C = new Mat22();C.set(this.mul(R.ex), this.mul(R.ey));return C; + */ + val C = Mat22() + C.ex.x = ex.x * R.ex.x + ey.x * R.ex.y + C.ex.y = ex.y * R.ex.x + ey.y * R.ex.y + C.ey.x = ex.x * R.ey.x + ey.x * R.ey.y + C.ey.y = ex.y * R.ey.x + ey.y * R.ey.y + // C.set(ex,col2); + return C + } + + fun mulLocal(R: Mat22): Mat22 { + mulToOut(R, this) + return this + } + + fun mulToOut(R: Mat22, out: Mat22) { + val tempy1 = this.ex.y * R.ex.x + this.ey.y * R.ex.y + val tempx1 = this.ex.x * R.ex.x + this.ey.x * R.ex.y + out.ex.x = tempx1 + out.ex.y = tempy1 + val tempy2 = this.ex.y * R.ey.x + this.ey.y * R.ey.y + val tempx2 = this.ex.x * R.ey.x + this.ey.x * R.ey.y + out.ey.x = tempx2 + out.ey.y = tempy2 + } + + fun mulToOutUnsafe(R: Mat22, out: Mat22) { + assert(out !== R) + assert(out !== this) + out.ex.x = this.ex.x * R.ex.x + this.ey.x * R.ex.y + out.ex.y = this.ex.y * R.ex.x + this.ey.y * R.ex.y + out.ey.x = this.ex.x * R.ey.x + this.ey.x * R.ey.y + out.ey.y = this.ex.y * R.ey.x + this.ey.y * R.ey.y + } + + /** + * Multiply another matrix by the transpose of this one (transpose of this one on left). djm: + * optimized + * + * @param B + * @return + */ + fun mulTrans(B: Mat22): Mat22 { + /* + * Vec2 c1 = new Vec2(Vec2.dot(this.ex, B.ex), Vec2.dot(this.ey, B.ex)); Vec2 c2 = new + * Vec2(Vec2.dot(this.ex, B.ey), Vec2.dot(this.ey, B.ey)); Mat22 C = new Mat22(); C.set(c1, c2); + * return C; + */ + val C = Mat22() + + C.ex.x = Vec2.dot(this.ex!!, B.ex!!) + C.ex.y = Vec2.dot(this.ey!!, B.ex) + + C.ey.x = Vec2.dot(this.ex, B.ey!!) + C.ey.y = Vec2.dot(this.ey, B.ey) + return C + } + + fun mulTransLocal(B: Mat22): Mat22 { + mulTransToOut(B, this) + return this + } + + fun mulTransToOut(B: Mat22, out: Mat22) { + /* + * out.ex.x = Vec2.dot(this.ex, B.ex); out.ex.y = Vec2.dot(this.ey, B.ex); out.ey.x = + * Vec2.dot(this.ex, B.ey); out.ey.y = Vec2.dot(this.ey, B.ey); + */ + val x1 = this.ex.x * B.ex.x + this.ex.y * B.ex.y + val y1 = this.ey.x * B.ex.x + this.ey.y * B.ex.y + val x2 = this.ex.x * B.ey.x + this.ex.y * B.ey.y + val y2 = this.ey.x * B.ey.x + this.ey.y * B.ey.y + out.ex.x = x1 + out.ey.x = x2 + out.ex.y = y1 + out.ey.y = y2 + } + + fun mulTransToOutUnsafe(B: Mat22, out: Mat22) { + assert(B !== out) + assert(this !== out) + out.ex.x = this.ex.x * B.ex.x + this.ex.y * B.ex.y + out.ey.x = this.ex.x * B.ey.x + this.ex.y * B.ey.y + out.ex.y = this.ey.x * B.ex.x + this.ey.y * B.ex.y + out.ey.y = this.ey.x * B.ey.x + this.ey.y * B.ey.y + } + + /** + * Multiply a vector by the transpose of this matrix. + * + * @param v + * @return + */ + fun mulTrans(v: Vec2): Vec2 { + // return new Vec2(Vec2.dot(v, ex), Vec2.dot(v, col2)); + return Vec2(v.x * ex.x + v.y * ex.y, v.x * ey.x + v.y * ey.y) + } + + /* djm added */ + fun mulTransToOut(v: Vec2, out: Vec2) { + /* + * out.x = Vec2.dot(v, ex); out.y = Vec2.dot(v, col2); + */ + val tempx = v.x * ex.x + v.y * ex.y + out.y = v.x * ey.x + v.y * ey.y + out.x = tempx + } + + /** + * Add this matrix to B, return the result. + * + * @param B + * @return + */ + fun add(B: Mat22): Mat22 { + // return new Mat22(ex.add(B.ex), col2.add(B.ey)); + val m = Mat22() + m.ex.x = ex.x + B.ex.x + m.ex.y = ex.y + B.ex.y + m.ey.x = ey.x + B.ey.x + m.ey.y = ey.y + B.ey.y + return m + } + + /** + * Add B to this matrix locally. + * + * @param B + * @return + */ + fun addLocal(B: Mat22): Mat22 { + // ex.addLocal(B.ex); + // col2.addLocal(B.ey); + ex.x += B.ex.x + ex.y += B.ex.y + ey.x += B.ey.x + ey.y += B.ey.y + return this + } + + /** + * Solve A * x = b where A = this matrix. + * + * @return The vector x that solves the above equation. + */ + fun solve(b: Vec2): Vec2 { + val a11 = ex.x + val a12 = ey.x + val a21 = ex.y + val a22 = ey.y + var det = a11 * a22 - a12 * a21 + if (det != 0.0f) { + det = 1.0f / det + } + val x = Vec2(det * (a22 * b.x - a12 * b.y), det * (a11 * b.y - a21 * b.x)) + return x + } + + fun solveToOut(b: Vec2, out: Vec2) { + val a11 = ex.x + val a12 = ey.x + val a21 = ex.y + val a22 = ey.y + var det = a11 * a22 - a12 * a21 + if (det != 0.0f) { + det = 1.0f / det + } + val tempy = det * (a11 * b.y - a21 * b.x) + out.x = det * (a22 * b.x - a12 * b.y) + out.y = tempy + } + + override fun hashCode(): Int { + val prime = 31 + var result = 1 + result = prime * result + (ex?.hashCode() ?: 0) + result = prime * result + (ey?.hashCode() ?: 0) + return result + } + + override fun equals(obj: Any?): Boolean { + if (this === obj) return true + if (obj == null) return false + if (this::class != obj::class) return false + val other = obj as Mat22? + if (ex == null) { + if (other?.ex != null) return false + } else if (ex != other?.ex) return false + if (ey == null) { + if (other?.ey != null) return false + } else if (ey != other?.ey) return false + return true + } + + companion object { + /** + * Return the matrix composed of the absolute values of all elements. + * + * @return Absolute value matrix + */ + + fun abs(R: Mat22): Mat22 { + return R.abs() + } + + /* djm created */ + + fun absToOut(R: Mat22, out: Mat22) { + out.ex.x = MathUtils.abs(R.ex.x) + out.ex.y = MathUtils.abs(R.ex.y) + out.ey.x = MathUtils.abs(R.ey.x) + out.ey.y = MathUtils.abs(R.ey.y) + } + + + fun mul(R: Mat22, v: Vec2): Vec2 { + // return R.mul(v); + return Vec2(R.ex.x * v.x + R.ey.x * v.y, R.ex.y * v.x + R.ey.y * v.y) + } + + + fun mulToOut(R: Mat22, v: Vec2, out: Vec2) { + val tempy = R.ex.y * v.x + R.ey.y * v.y + out.x = R.ex.x * v.x + R.ey.x * v.y + out.y = tempy + } + + + fun mulToOutUnsafe(R: Mat22, v: Vec2, out: Vec2) { + assert(v !== out) + out.x = R.ex.x * v.x + R.ey.x * v.y + out.y = R.ex.y * v.x + R.ey.y * v.y + } + + + fun mul(A: Mat22, B: Mat22): Mat22 { + // return A.mul(B); + val C = Mat22() + C.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y + C.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y + C.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y + C.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y + return C + } + + + fun mulToOut(A: Mat22, B: Mat22, out: Mat22) { + val tempy1 = A.ex.y * B.ex.x + A.ey.y * B.ex.y + val tempx1 = A.ex.x * B.ex.x + A.ey.x * B.ex.y + val tempy2 = A.ex.y * B.ey.x + A.ey.y * B.ey.y + val tempx2 = A.ex.x * B.ey.x + A.ey.x * B.ey.y + out.ex.x = tempx1 + out.ex.y = tempy1 + out.ey.x = tempx2 + out.ey.y = tempy2 + } + + + fun mulToOutUnsafe(A: Mat22, B: Mat22, out: Mat22) { + assert(out !== A) + assert(out !== B) + out.ex.x = A.ex.x * B.ex.x + A.ey.x * B.ex.y + out.ex.y = A.ex.y * B.ex.x + A.ey.y * B.ex.y + out.ey.x = A.ex.x * B.ey.x + A.ey.x * B.ey.y + out.ey.y = A.ex.y * B.ey.x + A.ey.y * B.ey.y + } + + + fun mulTrans(R: Mat22, v: Vec2): Vec2 { + return Vec2(v.x * R.ex.x + v.y * R.ex.y, v.x * R.ey.x + v.y * R.ey.y) + } + + + fun mulTransToOut(R: Mat22, v: Vec2, out: Vec2) { + val outx = v.x * R.ex.x + v.y * R.ex.y + out.y = v.x * R.ey.x + v.y * R.ey.y + out.x = outx + } + + + fun mulTransToOutUnsafe(R: Mat22, v: Vec2, out: Vec2) { + assert(out !== v) + out.y = v.x * R.ey.x + v.y * R.ey.y + out.x = v.x * R.ex.x + v.y * R.ex.y + } + + + fun mulTrans(A: Mat22, B: Mat22): Mat22 { + val C = Mat22() + C.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y + C.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y + C.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y + C.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y + return C + } + + + fun mulTransToOut(A: Mat22, B: Mat22, out: Mat22) { + val x1 = A.ex.x * B.ex.x + A.ex.y * B.ex.y + val y1 = A.ey.x * B.ex.x + A.ey.y * B.ex.y + val x2 = A.ex.x * B.ey.x + A.ex.y * B.ey.y + val y2 = A.ey.x * B.ey.x + A.ey.y * B.ey.y + + out.ex.x = x1 + out.ex.y = y1 + out.ey.x = x2 + out.ey.y = y2 + } + + + fun mulTransToOutUnsafe(A: Mat22, B: Mat22, out: Mat22) { + assert(A !== out) + assert(B !== out) + out.ex.x = A.ex.x * B.ex.x + A.ex.y * B.ex.y + out.ex.y = A.ey.x * B.ex.x + A.ey.y * B.ex.y + out.ey.x = A.ex.x * B.ey.x + A.ex.y * B.ey.y + out.ey.y = A.ey.x * B.ey.x + A.ey.y * B.ey.y + } + + + fun createRotationalTransformRadians(angleRadians: Float, out: Mat22 = Mat22()): Mat22 { + val c = MathUtils.cos(angleRadians) + val s = MathUtils.sin(angleRadians) + out.ex.x = c + out.ey.x = -s + out.ex.y = s + out.ey.y = c + return out + } + + fun createRotationalTransformDegrees(angleDegrees: Float, out: Mat22 = Mat22()): Mat22 + = createRotationalTransformRadians(angleDegrees * MathUtils.DEG2RAD, out) + + fun createRotationalTransform(angle: Angle, out: Mat22 = Mat22()): Mat22 + = createRotationalTransformRadians(angle.radians.toFloat(), out) + + + fun createScaleTransform(scale: Float): Mat22 { + val mat = Mat22() + mat.ex.x = scale + mat.ey.y = scale + return mat + } + + + fun createScaleTransform(scale: Float, out: Mat22) { + out.ex.x = scale + out.ey.y = scale + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat33.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat33.kt new file mode 100644 index 0000000..7bc7781 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Mat33.kt @@ -0,0 +1,309 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import org.jbox2d.internal.assert + +/** + * A 3-by-3 matrix. Stored in column-major order. + * + * @author Daniel Murphy + */ +class Mat33 { + + + val ex: Vec3 + + val ey: Vec3 + + val ez: Vec3 + + constructor() { + ex = Vec3() + ey = Vec3() + ez = Vec3() + } + + constructor(exx: Float, exy: Float, exz: Float, eyx: Float, eyy: Float, eyz: Float, ezx: Float, + ezy: Float, ezz: Float) { + ex = Vec3(exx, exy, exz) + ey = Vec3(eyx, eyy, eyz) + ez = Vec3(ezx, ezy, ezz) + } + + constructor(argCol1: Vec3, argCol2: Vec3, argCol3: Vec3) { + ex = argCol1.clone() + ey = argCol2.clone() + ez = argCol3.clone() + } + + fun setZero() { + ex.setZero() + ey.setZero() + ez.setZero() + } + + fun set(exx: Float, exy: Float, exz: Float, eyx: Float, eyy: Float, eyz: Float, ezx: Float, + ezy: Float, ezz: Float) { + ex.x = exx + ex.y = exy + ex.z = exz + ey.x = eyx + ey.y = eyy + ey.z = eyz + ez.x = eyx + ez.y = eyy + ez.z = eyz + } + + fun set(mat: Mat33) { + val vec = mat.ex + ex.x = vec.x + ex.y = vec.y + ex.z = vec.z + val vec1 = mat.ey + ey.x = vec1.x + ey.y = vec1.y + ey.z = vec1.z + val vec2 = mat.ez + ez.x = vec2.x + ez.y = vec2.y + ez.z = vec2.z + } + + fun setIdentity() { + ex.x = 1.toFloat() + ex.y = 0.toFloat() + ex.z = 0.toFloat() + ey.x = 0.toFloat() + ey.y = 1.toFloat() + ey.z = 0.toFloat() + ez.x = 0.toFloat() + ez.y = 0.toFloat() + ez.z = 1.toFloat() + } + + /** + * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse + * in one-shot cases. + * + * @param b + * @return + */ + fun solve22(b: Vec2): Vec2 { + val x = Vec2() + solve22ToOut(b, x) + return x + } + + /** + * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse + * in one-shot cases. + * + * @param b + * @return + */ + fun solve22ToOut(b: Vec2, out: Vec2) { + val a11 = ex.x + val a12 = ey.x + val a21 = ex.y + val a22 = ey.y + var det = a11 * a22 - a12 * a21 + if (det != 0.0f) { + det = 1.0f / det + } + out.x = det * (a22 * b.x - a12 * b.y) + out.y = det * (a11 * b.y - a21 * b.x) + } + + // djm pooling from below + /** + * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse + * in one-shot cases. + * + * @param b + * @return + */ + fun solve33(b: Vec3): Vec3 { + val x = Vec3() + solve33ToOut(b, x) + return x + } + + /** + * Solve A * x = b, where b is a column vector. This is more efficient than computing the inverse + * in one-shot cases. + * + * @param b + * @param out the result + */ + fun solve33ToOut(b: Vec3, out: Vec3) { + assert(b !== out) + Vec3.crossToOutUnsafe(ey!!, ez!!, out) + var det = Vec3.dot(ex!!, out) + if (det != 0.0f) { + det = 1.0f / det + } + Vec3.crossToOutUnsafe(ey, ez, out) + val x = det * Vec3.dot(b, out) + Vec3.crossToOutUnsafe(b, ez, out) + val y = det * Vec3.dot(ex, out) + Vec3.crossToOutUnsafe(ey, b, out) + val z = det * Vec3.dot(ex, out) + out.x = x + out.y = y + out.z = z + } + + fun getInverse22(M: Mat33) { + val a = ex.x + val b = ey.x + val c = ex.y + val d = ey.y + var det = a * d - b * c + if (det != 0.0f) { + det = 1.0f / det + } + + M.ex.x = det * d + M.ey.x = -det * b + M.ex.z = 0.0f + M.ex.y = -det * c + M.ey.y = det * a + M.ey.z = 0.0f + M.ez.x = 0.0f + M.ez.y = 0.0f + M.ez.z = 0.0f + } + + // / Returns the zero matrix if singular. + fun getSymInverse33(M: Mat33) { + val bx = ey.y * ez.z - ey.z * ez.y + val by = ey.z * ez.x - ey.x * ez.z + val bz = ey.x * ez.y - ey.y * ez.x + var det = ex.x * bx + ex.y * by + ex.z * bz + if (det != 0.0f) { + det = 1.0f / det + } + + val a11 = ex.x + val a12 = ey.x + val a13 = ez.x + val a22 = ey.y + val a23 = ez.y + val a33 = ez.z + + M.ex.x = det * (a22 * a33 - a23 * a23) + M.ex.y = det * (a13 * a23 - a12 * a33) + M.ex.z = det * (a12 * a23 - a13 * a22) + + M.ey.x = M.ex.y + M.ey.y = det * (a11 * a33 - a13 * a13) + M.ey.z = det * (a13 * a12 - a11 * a23) + + M.ez.x = M.ex.z + M.ez.y = M.ey.z + M.ez.z = det * (a11 * a22 - a12 * a12) + } + + override fun hashCode(): Int { + val prime = 31 + var result = 1 + result = prime * result + (ex?.hashCode() ?: 0) + result = prime * result + (ey?.hashCode() ?: 0) + result = prime * result + (ez?.hashCode() ?: 0) + return result + } + + override fun equals(obj: Any?): Boolean { + if (this === obj) return true + if (obj == null) return false + if (this::class != obj::class) return false + val other = obj as Mat33? + if (ex == null) { + if (other?.ex != null) return false + } else if (ex != other?.ex) return false + if (ey == null) { + if (other?.ey != null) return false + } else if (ey != other?.ey) return false + if (ez == null) { + if (other?.ez != null) return false + } else if (ez != other?.ez) return false + return true + } + + companion object { + + //@ThreadLocal + val IDENTITY = Mat33(Vec3(1f, 0f, 0f), Vec3(0f, 1f, 0f), Vec3(0f, 0f, 1f)) + + // / Multiply a matrix times a vector. + + fun mul(A: Mat33, v: Vec3): Vec3 { + return Vec3(v.x * A.ex.x + v.y * A.ey.x + v.z + A.ez.x, v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y, v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z) + } + + + fun mul22(A: Mat33, v: Vec2): Vec2 { + return Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y) + } + + + fun mul22ToOut(A: Mat33, v: Vec2, out: Vec2) { + val tempx = A.ex.x * v.x + A.ey.x * v.y + out.y = A.ex.y * v.x + A.ey.y * v.y + out.x = tempx + } + + + fun mul22ToOutUnsafe(A: Mat33, v: Vec2, out: Vec2) { + assert(v !== out) + out.y = A.ex.y * v.x + A.ey.y * v.y + out.x = A.ex.x * v.x + A.ey.x * v.y + } + + + fun mulToOut(A: Mat33, v: Vec3, out: Vec3) { + val tempy = v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y + val tempz = v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z + out.x = v.x * A.ex.x + v.y * A.ey.x + v.z * A.ez.x + out.y = tempy + out.z = tempz + } + + + fun mulToOutUnsafe(A: Mat33, v: Vec3, out: Vec3) { + assert(out !== v) + out.x = v.x * A.ex.x + v.y * A.ey.x + v.z * A.ez.x + out.y = v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y + out.z = v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z + } + + + fun setScaleTransform(scale: Float, out: Mat33) { + out.ex.x = scale + out.ey.y = scale + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/MathUtils.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/MathUtils.kt new file mode 100644 index 0000000..98b0773 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/MathUtils.kt @@ -0,0 +1,328 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.common + +import kotlin.math.ceil +import kotlin.math.floor +import kotlin.math.pow +import kotlin.math.round +import kotlin.random.Random + +/** + * A few math methods that don't fit very well anywhere else. + */ +class MathUtils : PlatformMathUtils() { + companion object { + + val PI = kotlin.math.PI.toFloat() + + val TWOPI = (kotlin.math.PI * 2).toFloat() + + val INV_PI = 1f / PI + + val HALF_PI = PI / 2 + + val QUARTER_PI = PI / 4 + + val THREE_HALVES_PI = TWOPI - HALF_PI + + /** + * Degrees to radians conversion factor + */ + + val DEG2RAD = PI / 180 + + /** + * Radians to degrees conversion factor + */ + + val RAD2DEG = 180 / PI + + + val sinLUT = FloatArray(Settings.SINCOS_LUT_LENGTH) { + kotlin.math.sin((it * Settings.SINCOS_LUT_PRECISION).toDouble()).toFloat() + } + + fun sin(x: Float): Float { + return if (Settings.SINCOS_LUT_ENABLED) { + sinLUT(x) + } else { + kotlin.math.sin(x.toDouble()).toFloat() + } + } + + + fun sinLUT(x: Float): Float { + var x = x + x %= TWOPI + + if (x < 0) { + x += TWOPI + } + + if (Settings.SINCOS_LUT_LERP) { + + x /= Settings.SINCOS_LUT_PRECISION + + val index = x.toInt() + + if (index != 0) { + x %= index.toFloat() + } + + // the next index is 0 + return if (index == Settings.SINCOS_LUT_LENGTH - 1) { + (1 - x) * sinLUT[index] + x * sinLUT[0] + } else { + (1 - x) * sinLUT[index] + x * sinLUT[index + 1] + } + + } else { + return sinLUT[MathUtils.round(x / Settings.SINCOS_LUT_PRECISION) % Settings.SINCOS_LUT_LENGTH] + } + } + + + fun cos(x: Float): Float { + return if (Settings.SINCOS_LUT_ENABLED) { + sinLUT(HALF_PI - x) + } else { + kotlin.math.cos(x.toDouble()).toFloat() + } + } + + + fun abs(x: Float): Float { + return if (Settings.FAST_ABS) { + if (x > 0) x else -x + } else { + kotlin.math.abs(x) + } + } + + + fun fastAbs(x: Float): Float { + return if (x > 0) x else -x + } + + + fun abs(x: Int): Int { + val y = x shr 31 + return (x xor y) - y + } + + + fun floor(x: Float): Int = if (Settings.FAST_FLOOR) fastFloor(x) else floor(x.toDouble()).toInt() + + + fun fastFloor(x: Float): Int { + val y = x.toInt() + return if (x < y) y - 1 else y + } + + + fun ceil(x: Float): Int = if (Settings.FAST_CEIL) fastCeil(x) else ceil(x.toDouble()).toInt() + + + fun fastCeil(x: Float): Int { + val y = x.toInt() + return if (x > y) y + 1 else y + } + + + fun round(x: Float): Int = if (Settings.FAST_ROUND) floor(x + .5f) else round(x.toDouble()).toInt() + + /** + * Rounds up the value to the nearest higher power^2 value. + * + * @param x + * @return power^2 value + */ + + fun ceilPowerOf2(x: Int): Int { + var pow2 = 1 + while (pow2 < x) pow2 = pow2 shl 1 + return pow2 + } + + + fun max(a: Float, b: Float): Float = if (a > b) a else b + fun max(a: Int, b: Int): Int = if (a > b) a else b + fun min(a: Float, b: Float): Float = if (a < b) a else b + fun min(a: Int, b: Int): Int = if (a < b) a else b + + fun map(`val`: Float, fromMin: Float, fromMax: Float, + toMin: Float, toMax: Float): Float { + val mult = (`val` - fromMin) / (fromMax - fromMin) + val res = toMin + mult * (toMax - toMin) + return res + } + + /** Returns the closest value to 'a' that is in between 'low' and 'high' */ + + fun clamp(a: Float, low: Float, high: Float): Float = max(low, min(a, high)) + + + fun clamp(a: Vec2, low: Vec2, high: Vec2): Vec2 { + val min = Vec2() + min.x = if (a.x < high.x) a.x else high.x + min.y = if (a.y < high.y) a.y else high.y + min.x = if (low.x > min.x) low.x else min.x + min.y = if (low.y > min.y) low.y else min.y + return min + } + + + fun clampToOut(a: Vec2, low: Vec2, high: Vec2, dest: Vec2) { + dest.x = if (a.x < high.x) a.x else high.x + dest.y = if (a.y < high.y) a.y else high.y + dest.x = if (low.x > dest.x) low.x else dest.x + dest.y = if (low.y > dest.y) low.y else dest.y + } + + /** + * Next Largest Power of 2: Given a binary integer value x, the next largest power of 2 can be + * computed by a SWAR algorithm that recursively "folds" the upper bits into the lower bits. This + * process yields a bit vector with the same most significant 1 as x, but all 1's below it. Adding + * 1 to that value yields the next largest power of 2. + */ + + fun nextPowerOfTwo(x: Int): Int { + var x = x + x = x or (x shr 1) + x = x or (x shr 2) + x = x or (x shr 4) + x = x or (x shr 8) + x = x or (x shr 16) + return x + 1 + } + + + fun isPowerOfTwo(x: Int): Boolean { + return x > 0 && x and x - 1 == 0 + } + + + fun pow(a: Float, b: Float): Float { + return if (Settings.FAST_POW) { + PlatformMathUtils.fastPow(a, b) + } else { + a.toDouble().pow(b.toDouble()).toFloat() + } + } + + + fun atan2(y: Float, x: Float): Float { + return if (Settings.FAST_ATAN2) { + fastAtan2(y, x) + } else { + kotlin.math.atan2(y.toDouble(), x.toDouble()).toFloat() + } + } + + + fun fastAtan2(y: Float, x: Float): Float { + if (x == 0.0f) { + if (y > 0.0f) return HALF_PI + return if (y == 0.0f) 0.0f else -HALF_PI + } + val atan: Float + val z = y / x + if (abs(z) < 1.0f) { + atan = z / (1.0f + 0.28f * z * z) + if (x < 0.0f) { + return if (y < 0.0f) atan - PI else atan + PI + } + } else { + atan = HALF_PI - z / (z * z + 0.28f) + if (y < 0.0f) return atan - PI + } + return atan + } + + + fun reduceAngle(theta: Float): Float { + var theta = theta + theta %= TWOPI + if (abs(theta) > PI) { + theta -= TWOPI + } + if (abs(theta) > HALF_PI) { + theta = PI - theta + } + return theta + } + + + fun randomFloat(argLow: Float, argHigh: Float): Float { + + return kotlin.random.Random.nextFloat() * (argHigh - argLow) + argLow + } + + + fun randomFloat(r: Random, argLow: Float, argHigh: Float): Float { + return r.nextFloat() * (argHigh - argLow) + argLow + } + + + fun sqrt(x: Float): Float { + return kotlin.math.sqrt(x.toDouble()).toFloat() + } + + + fun distanceSquared(v1: Vec2, v2: Vec2): Float { + val dx = v1.x - v2.x + val dy = v1.y - v2.y + return dx * dx + dy * dy + } + + + fun distance(v1: Vec2, v2: Vec2): Float { + return sqrt(distanceSquared(v1, v2)) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/OBBViewportTransform.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/OBBViewportTransform.kt new file mode 100644 index 0000000..e92669e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/OBBViewportTransform.kt @@ -0,0 +1,142 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +/** + * Orientated bounding box viewport transform + * + * @author Daniel Murphy + */ +class OBBViewportTransform : IViewportTransform { + + protected val box = OBB() + override var isYFlip = false + private val yFlipMat = Mat22(1f, 0f, 0f, -1f) + + override var extents: Vec2 + get() = box.extents + set(argExtents) { + box.extents.set(argExtents) + } + + override val mat22Representation: Mat22 + get() = box.R + + override var center: Vec2 + get() = box.center + set(argPos) { + box.center.set(argPos) + } + + /** + * Gets the transform of the viewport, transforms around the center. Not a copy. + */ + /** + * Sets the transform of the viewport. Transforms about the center. + */ + var transform: Mat22 + get() = box.R + set(transform) { + box.R.set(transform) + } + + private val inv = Mat22() + + private val inv2 = Mat22() + + class OBB { + val R = Mat22() + val center = Vec2() + val extents = Vec2() + } + + init { + box.R.setIdentity() + } + + fun set(vpt: OBBViewportTransform) { + box.center.set(vpt.box.center) + box.extents.set(vpt.box.extents) + box.R.set(vpt.box.R) + isYFlip = vpt.isYFlip + } + + override fun setCamera(x: Float, y: Float, scale: Float) { + box.center.set(x, y) + Mat22.createScaleTransform(scale, box.R) + } + + override fun setExtents(halfWidth: Float, halfHeight: Float) { + box.extents.set(halfWidth, halfHeight) + } + + override fun setCenter(x: Float, y: Float) { + box.center.set(x, y) + } + + /** + * Multiplies the obb transform by the given transform + */ + override fun mulByTransform(transform: Mat22) { + box.R.mulLocal(transform) + } + + override fun getScreenVectorToWorld(screen: Vec2, world: Vec2) { + box.R.invertToOut(inv) + inv.mulToOut(screen, world) + if (isYFlip) { + yFlipMat.mulToOut(world, world) + } + } + + override fun getWorldVectorToScreen(world: Vec2, screen: Vec2) { + box.R.mulToOut(world, screen) + if (isYFlip) { + yFlipMat.mulToOut(screen, screen) + } + } + + override fun getWorldToScreen(world: Vec2, screen: Vec2) { + screen.x = world.x - box.center.x + screen.y = world.y - box.center.y + box.R.mulToOut(screen, screen) + if (isYFlip) { + yFlipMat.mulToOut(screen, screen) + } + screen.x += box.extents.x + screen.y += box.extents.y + } + + override fun getScreenToWorld(screen: Vec2, world: Vec2) { + world.x = screen.x - box.extents.x + world.y = screen.y - box.extents.y + if (isYFlip) { + yFlipMat.mulToOut(world, world) + } + box.R.invertToOut(inv2) + inv2.mulToOut(world, world) + world.x += box.center.x + world.y += box.center.y + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/PlatformMathUtils.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/PlatformMathUtils.kt new file mode 100644 index 0000000..34bb397 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/PlatformMathUtils.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +/** + * Contains methods from MathUtils that rely on JVM features. These are separated out from + * MathUtils so that they can be overridden when compiling for GWT. + */ +open class PlatformMathUtils { + companion object { + private val SHIFT23 = (1 shl 23).toFloat() + private val INV_SHIFT23 = 1.0f / SHIFT23 + + fun fastPow(a: Float, b: Float): Float { + var b = b + var x = a.toRawBits().toFloat() + x *= INV_SHIFT23 + x -= 127f + var y = x - if (x >= 0) x.toInt() else x.toInt() - 1 + b *= x + (y - y * y) * 0.346607f + y = b - if (b >= 0) b.toInt() else b.toInt() - 1 + y = (y - y * y) * 0.33971f + return Float.fromBits(((b + 127 - y) * SHIFT23).toInt()) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/RaycastResult.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/RaycastResult.kt new file mode 100644 index 0000000..423230e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/RaycastResult.kt @@ -0,0 +1,39 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +// updated to rev 100 + +class RaycastResult { + + var lambda = 0.0f + + val normal = Vec2() + + fun set(argOther: RaycastResult): RaycastResult { + lambda = argOther.lambda + normal.set(argOther.normal) + return this + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Rot.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Rot.kt new file mode 100644 index 0000000..49dd2bb --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Rot.kt @@ -0,0 +1,161 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.internal.assert + +/** + * Represents a rotation + * + * @author Daniel + */ +class Rot { + + + var s: Float = 0.toFloat() + + var c: Float = 0.toFloat() // sin and cos + + val angleRadians: Float get() = MathUtils.atan2(s, c) + + val angleDegrees: Float get() = angleRadians * MathUtils.RAD2DEG + + val angle: Angle get() = angleRadians.radians + + constructor() { + setIdentity() + } + + constructor(angle: Angle) { + set(angle) + } + + override fun toString(): String { + return "Rot(s:$s, c:$c)" + } + + fun setRadians(angleRadians: Float): Rot { + s = MathUtils.sin(angleRadians) + c = MathUtils.cos(angleRadians) + return this + } + + fun set(angle: Angle): Rot = setRadians(angle.radians.toFloat()) + + fun setDegrees(angleDegrees: Float): Rot = setRadians(angleDegrees * MathUtils.DEG2RAD) + + fun set(other: Rot): Rot { + s = other.s + c = other.c + return this + } + + fun setIdentity(): Rot { + s = 0f + c = 1f + return this + } + + fun getXAxis(xAxis: Vec2) { + xAxis.set(c, s) + } + + fun getYAxis(yAxis: Vec2) { + yAxis.set(-s, c) + } + + // @Override // annotation omitted for GWT-compatibility + fun clone(): Rot { + val copy = Rot() + copy.s = s + copy.c = c + return copy + } + + companion object { + + fun mul(q: Rot, r: Rot, out: Rot) { + val tempc = q.c * r.c - q.s * r.s + out.s = q.s * r.c + q.c * r.s + out.c = tempc + } + + + fun mulUnsafe(q: Rot, r: Rot, out: Rot) { + assert(r !== out) + assert(q !== out) + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + out.s = q.s * r.c + q.c * r.s + out.c = q.c * r.c - q.s * r.s + } + + + fun mulTrans(q: Rot, r: Rot, out: Rot) { + val tempc = q.c * r.c + q.s * r.s + out.s = q.c * r.s - q.s * r.c + out.c = tempc + } + + + fun mulTransUnsafe(q: Rot, r: Rot, out: Rot) { + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + out.s = q.c * r.s - q.s * r.c + out.c = q.c * r.c + q.s * r.s + } + + + fun mulToOut(q: Rot, v: Vec2, out: Vec2) { + val tempy = q.s * v.x + q.c * v.y + out.x = q.c * v.x - q.s * v.y + out.y = tempy + } + + + fun mulToOutUnsafe(q: Rot, v: Vec2, out: Vec2) { + out.x = q.c * v.x - q.s * v.y + out.y = q.s * v.x + q.c * v.y + } + + + fun mulTrans(q: Rot, v: Vec2, out: Vec2) { + val tempy = -q.s * v.x + q.c * v.y + out.x = q.c * v.x + q.s * v.y + out.y = tempy + } + + + fun mulTransUnsafe(q: Rot, v: Vec2, out: Vec2) { + out.x = q.c * v.x + q.s * v.y + out.y = -q.s * v.x + q.c * v.y + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Settings.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Settings.kt new file mode 100644 index 0000000..27d27d9 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Settings.kt @@ -0,0 +1,382 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import kotlin.native.concurrent.* + +@ThreadLocal private var _FAST_ABS = true +@ThreadLocal private var _FAST_FLOOR = true +@ThreadLocal private var _FAST_CEIL = true +@ThreadLocal private var _FAST_ROUND = false +@ThreadLocal private var _FAST_ATAN2 = true +@ThreadLocal private var _FAST_POW = true +@ThreadLocal private var _CONTACT_STACK_INIT_SIZE = 10 +@ThreadLocal private var _SINCOS_LUT_ENABLED = true +@ThreadLocal private var _SINCOS_LUT_LERP = false +@ThreadLocal private var _maxManifoldPoints = 2 +@ThreadLocal private var _maxPolygonVertices = 8 +@ThreadLocal private var _aabbExtension = 0.1f +@ThreadLocal private var _aabbMultiplier = 2.0f +@ThreadLocal private var _linearSlop = 0.005f +@ThreadLocal private var _angularSlop = 2.0f / 180.0f * Settings.PI +@ThreadLocal private var _polygonRadius = 2.0f * _linearSlop +@ThreadLocal private var _maxSubSteps = 8 +@ThreadLocal private var _maxTOIContacts = 32 +@ThreadLocal private var _velocityThreshold = 1.0f +@ThreadLocal private var _maxLinearCorrection = 0.2f +@ThreadLocal private var _maxAngularCorrection = 8.0f / 180.0f * Settings.PI +@ThreadLocal private var _maxTranslation = 2.0f +@ThreadLocal private var _maxTranslationSquared = _maxTranslation * _maxTranslation +@ThreadLocal private var _maxRotation = 0.5f * Settings.PI +@ThreadLocal private var _maxRotationSquared = _maxRotation * _maxRotation +@ThreadLocal private var _baumgarte = 0.2f +@ThreadLocal private var _toiBaugarte = 0.75f +@ThreadLocal private var _timeToSleep = 0.5f +@ThreadLocal private var _linearSleepTolerance = 0.01f +@ThreadLocal private var _angularSleepTolerance = 2.0f / 180.0f * Settings.PI + +/** + * Global tuning constants based on MKS units and various integer maximums (vertices per shape, + * pairs, etc.). + */ +object Settings { + + /** A "close to zero" float epsilon value for use */ + + val EPSILON = 1.1920928955078125E-7f + + /** Pi. */ + + val PI = kotlin.math.PI.toFloat() + + // JBox2D specific settings + + var FAST_ABS: Boolean + get() = _FAST_ABS + set(value) { _FAST_ABS = value } + + var FAST_FLOOR: Boolean + get() = _FAST_FLOOR + set(value) { _FAST_FLOOR = value } + + var FAST_CEIL + get() = _FAST_CEIL + set(value) { _FAST_CEIL = value } + + var FAST_ROUND + get() = _FAST_ROUND + set(value) { _FAST_ROUND = value } + + var FAST_ATAN2 + get() = _FAST_ATAN2 + set(value) { _FAST_ATAN2 = value } + + var FAST_POW + get() = _FAST_POW + set(value) { _FAST_POW = value } + + var CONTACT_STACK_INIT_SIZE + get() = _CONTACT_STACK_INIT_SIZE + set(value) { _CONTACT_STACK_INIT_SIZE = value } + + var SINCOS_LUT_ENABLED + get() = _SINCOS_LUT_ENABLED + set(value) { _SINCOS_LUT_ENABLED = value } + /** + * smaller the precision, the larger the table. If a small table is used (eg, precision is .006 or + * greater), make sure you set the table to lerp it's results. Accuracy chart is in the MathUtils + * source. Or, run the tests yourself in [SinCosTest]. Good lerp precision + * values: + * + * * .0092 + * * .008201 + * * .005904 + * * .005204 + * * .004305 + * * .002807 + * * .001508 + * * 9.32500E-4 + * * 7.48000E-4 + * * 8.47000E-4 + * * .0005095 + * * .0001098 + * * 9.50499E-5 + * * 6.08500E-5 + * * 3.07000E-5 + * * 1.53999E-5 + * + */ + + val SINCOS_LUT_PRECISION = .00011f + + val SINCOS_LUT_LENGTH = kotlin.math.ceil(kotlin.math.PI * 2 / SINCOS_LUT_PRECISION).toInt() + /** + * Use if the table's precision is large (eg .006 or greater). Although it is more expensive, it + * greatly increases accuracy. Look in the MathUtils source for some test results on the accuracy + * and speed of lerp vs non lerp. Or, run the tests yourself in [SinCosTest]. + */ + + var SINCOS_LUT_LERP + get() = _SINCOS_LUT_LERP + set(value) { _SINCOS_LUT_LERP = value } + + + // Collision + + /** + * The maximum number of contact points between two convex shapes. + */ + + var maxManifoldPoints + get() = _maxManifoldPoints + set(value) { _maxManifoldPoints = value } + + /** + * The maximum number of vertices on a convex polygon. + */ + + var maxPolygonVertices + get() = _maxPolygonVertices + set(value) { _maxPolygonVertices = value } + + /** + * This is used to fatten AABBs in the dynamic tree. This allows proxies to move by a small amount + * without triggering a tree adjustment. This is in meters. + */ + + var aabbExtension + get() = _aabbExtension + set(value) { _aabbExtension = value } + + /** + * This is used to fatten AABBs in the dynamic tree. This is used to predict the future position + * based on the current displacement. This is a dimensionless multiplier. + */ + + var aabbMultiplier + get() = _aabbMultiplier + set(value) { _aabbMultiplier = value } + + /** + * A small length used as a collision and constraint tolerance. Usually it is chosen to be + * numerically significant, but visually insignificant. + */ + + var linearSlop + get() = _linearSlop + set(value) { _linearSlop = value } + + /** + * A small angle used as a collision and constraint tolerance. Usually it is chosen to be + * numerically significant, but visually insignificant. + */ + + var angularSlop + get() = _angularSlop + set(value) { _angularSlop = value } + + /** + * The radius of the polygon/edge shape skin. This should not be modified. Making this smaller + * means polygons will have and insufficient for continuous collision. Making it larger may create + * artifacts for vertex collision. + */ + + var polygonRadius + get() = _polygonRadius + set(value) { _polygonRadius = value } + + /** Maximum number of sub-steps per contact in continuous physics simulation. */ + + var maxSubSteps + get() = _maxSubSteps + set(value) { _maxSubSteps = value } + + // Dynamics + + /** + * Maximum number of contacts to be handled to solve a TOI island. + */ + + var maxTOIContacts + get() = _maxTOIContacts + set(value) { _maxTOIContacts = value } + + /** + * A velocity threshold for elastic collisions. Any collision with a relative linear velocity + * below this threshold will be treated as inelastic. + */ + + var velocityThreshold + get() = _velocityThreshold + set(value) { _velocityThreshold = value } + + /** + * The maximum linear position correction used when solving constraints. This helps to prevent + * overshoot. + */ + + var maxLinearCorrection + get() = _maxLinearCorrection + set(value) { _maxLinearCorrection = value } + + /** + * The maximum angular position correction used when solving constraints. This helps to prevent + * overshoot. + */ + + var maxAngularCorrection + get() = _maxAngularCorrection + set(value) { _maxAngularCorrection = value } + + /** + * The maximum linear velocity of a body. This limit is very large and is used to prevent + * numerical problems. You shouldn't need to adjust this. + */ + + var maxTranslation + get() = _maxTranslation + set(value) { _maxTranslation = value } + + var maxTranslationSquared + get() = _maxTranslationSquared + set(value) { _maxTranslationSquared = value } + + /** + * The maximum angular velocity of a body. This limit is very large and is used to prevent + * numerical problems. You shouldn't need to adjust this. + */ + + var maxRotation + get() = _maxRotation + set(value) { _maxRotation = value } + + var maxRotationSquared + get() = _maxRotationSquared + set(value) { _maxRotationSquared = value } + + /** + * This scale factor controls how fast overlap is resolved. Ideally this would be 1 so that + * overlap is removed in one time step. However using values close to 1 often lead to overshoot. + */ + + var baumgarte + get() = _baumgarte + set(value) { _baumgarte = value } + + var toiBaugarte + get() = _toiBaugarte + set(value) { _toiBaugarte = value } + + + // Sleep + + /** + * The time that a body must be still before it will go to sleep. + */ + + var timeToSleep + get() = _timeToSleep + set(value) { _timeToSleep = value } + + /** + * A body cannot sleep if its linear velocity is above this tolerance. + */ + + var linearSleepTolerance + get() = _linearSleepTolerance + set(value) { _linearSleepTolerance = value } + + /** + * A body cannot sleep if its angular velocity is above this tolerance. + */ + + var angularSleepTolerance + get() = _angularSleepTolerance + set(value) { _angularSleepTolerance = value } + + // Particle + + /** + * A symbolic constant that stands for particle allocation error. + */ + + val invalidParticleIndex = -1 + + /** + * The standard distance between particles, divided by the particle radius. + */ + + val particleStride = 0.75f + + /** + * The minimum particle weight that produces pressure. + */ + + val minParticleWeight = 1.0f + + /** + * The upper limit for particle weight used in pressure calculation. + */ + + val maxParticleWeight = 5.0f + + /** + * The maximum distance between particles in a triad, divided by the particle radius. + */ + + val maxTriadDistance = 2 + + val maxTriadDistanceSquared = maxTriadDistance * maxTriadDistance + + /** + * The initial size of particle data buffers. + */ + + val minParticleBufferCapacity = 256 + + + /** + * Friction mixing law. Feel free to customize this. TODO djm: add customization + * + * @param friction1 + * @param friction2 + * @return + */ + + fun mixFriction(friction1: Float, friction2: Float): Float { + return MathUtils.sqrt(friction1 * friction2) + } + + /** + * Restitution mixing law. Feel free to customize this. TODO djm: add customization + * + * @param restitution1 + * @param restitution2 + * @return + */ + + fun mixRestitution(restitution1: Float, restitution2: Float): Float { + return if (restitution1 > restitution2) restitution1 else restitution2 + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Sweep.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Sweep.kt new file mode 100644 index 0000000..4cdc39e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Sweep.kt @@ -0,0 +1,117 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import org.jbox2d.internal.* + +/** + * This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to + * the body origin, which may not coincide with the center of mass. However, to support dynamics we + * must interpolate the center of mass position. + */ +class Sweep { + + /** Local center of mass position */ + + val localCenter: Vec2 = Vec2() + /** Center world positions */ + + val c0: Vec2 = Vec2() + + val c: Vec2 = Vec2() + /** World angles */ + + var a0: Float = 0.toFloat() + + var a: Float = 0.toFloat() + + /** Fraction of the current time step in the range [0,1] c0 and a0 are the positions at alpha0. */ + + var alpha0: Float = 0.toFloat() + + override fun toString(): String { + var s = "Sweep:\nlocalCenter: $localCenter\n" + s += "c0: $c0, c: $c\n" + s += "a0: $a0, a: $a\n" + s += "alpha0: $alpha0" + return s + } + + fun normalize() { + val d = MathUtils.TWOPI * MathUtils.floor(a0 / MathUtils.TWOPI) + a0 -= d + a -= d + } + + fun set(other: Sweep): Sweep { + localCenter.set(other.localCenter) + c0.set(other.c0) + c.set(other.c) + a0 = other.a0 + a = other.a + alpha0 = other.alpha0 + return this + } + + /** + * Get the interpolated transform at a specific time. + * + * @param xf the result is placed here - must not be null + * @param t the normalized time in [0,1]. + */ + fun getTransform(xf: Transform, beta: Float) { + assert(xf != null) + // xf->p = (1.0f - beta) * c0 + beta * c; + // float32 angle = (1.0f - beta) * a0 + beta * a; + // xf->q.Set(angle); + xf.p.x = (1.0f - beta) * c0.x + beta * c.x + xf.p.y = (1.0f - beta) * c0.y + beta * c.y + val angle = (1.0f - beta) * a0 + beta * a + xf.q.setRadians(angle) + + // Shift to origin + // xf->p -= b2Mul(xf->q, localCenter); + val q = xf.q + xf.p.x -= q.c * localCenter.x - q.s * localCenter.y + xf.p.y -= q.s * localCenter.x + q.c * localCenter.y + } + + /** + * Advance the sweep forward, yielding a new initial state. + * + * @param alpha the new initial time. + */ + fun advance(alpha: Float) { + assert(alpha0 < 1.0f) + // float32 beta = (alpha - alpha0) / (1.0f - alpha0); + // c0 += beta * (c - c0); + // a0 += beta * (a - a0); + // alpha0 = alpha; + val beta = (alpha - alpha0) / (1.0f - alpha0) + c0.x += beta * (c.x - c0.x) + c0.y += beta * (c.y - c0.y) + a0 += beta * (a - a0) + alpha0 = alpha + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Timer.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Timer.kt new file mode 100644 index 0000000..404cca6 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Timer.kt @@ -0,0 +1,49 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import korlibs.time.* +import kotlin.time.* + +/** + * Timer for profiling + * + * @author Daniel + */ +class Timer { + + private var start = TimeSource.Monotonic.markNow() + + val milliseconds: Float + get() = start.elapsedNow().milliseconds.toFloat() + + init { + reset() + } + + fun reset() { + start = TimeSource.Monotonic.markNow() + } + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Transform.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Transform.kt new file mode 100644 index 0000000..771f01d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Transform.kt @@ -0,0 +1,206 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import korlibs.math.geom.* +import org.jbox2d.internal.* + +// updated to rev 100 + +/** + * A transform contains translation and rotation. It is used to represent the position and + * orientation of rigid frames. + */ +class Transform { + + /** The translation caused by the transform */ + + val p: Vec2 + + /** A matrix representing a rotation */ + + val q: Rot + + /** The default constructor. */ + constructor() { + p = Vec2() + q = Rot() + } + + /** Initialize as a copy of another transform. */ + constructor(xf: Transform) { + p = xf.p.clone() + q = xf.q.clone() + } + + /** Initialize using a position vector and a rotation matrix. */ + constructor(_position: Vec2, _R: Rot) { + p = _position.clone() + q = _R.clone() + } + + /** Set this to equal another transform. */ + fun set(xf: Transform): Transform { + p.set(xf.p) + q.set(xf.q) + return this + } + + /** + * Set this based on the position and angle. + * + * @param p + * @param angle + */ + fun set(p: Vec2, angle: Angle) = this.setRadians(p, angle.radians.toFloat()) + + /** + * Set this based on the position and angle in radians. + * + * @param p + * @param angleRadians + */ + fun setRadians(p: Vec2, angleRadians: Float) { + this.p.set(p) + q.setRadians(angleRadians) + } + + /** + * Set this based on the position and angle in degrees. + * + * @param p + * @param angle + */ + fun setDegrees(p: Vec2, angleDegrees: Float) = setRadians(p, angleDegrees * MathUtils.DEG2RAD) + + /** Set this to the identity transform. */ + fun setIdentity() { + p.setZero() + q.setIdentity() + } + + override fun toString(): String { + var s = "XForm:\n" + s += "Position: $p\n" + s += "R: \n$q\n" + return s + } + + companion object { + + fun mul(T: Transform, v: Vec2): Vec2 { + return Vec2(T.q.c * v.x - T.q.s * v.y + T.p.x, T.q.s * v.x + T.q.c * v.y + T.p.y) + } + + + fun mulToOut(T: Transform, v: Vec2, out: Vec2) { + val tempy = T.q.s * v.x + T.q.c * v.y + T.p.y + out.x = T.q.c * v.x - T.q.s * v.y + T.p.x + out.y = tempy + } + + + fun mulToOutUnsafe(T: Transform, v: Vec2, out: Vec2) { + assert(v !== out) + out.x = T.q.c * v.x - T.q.s * v.y + T.p.x + out.y = T.q.s * v.x + T.q.c * v.y + T.p.y + } + + + fun mulTrans(T: Transform, v: Vec2): Vec2 { + val px = v.x - T.p.x + val py = v.y - T.p.y + return Vec2(T.q.c * px + T.q.s * py, -T.q.s * px + T.q.c * py) + } + + + fun mulTransToOut(T: Transform, v: Vec2, out: Vec2) { + val px = v.x - T.p.x + val py = v.y - T.p.y + val tempy = -T.q.s * px + T.q.c * py + out.x = T.q.c * px + T.q.s * py + out.y = tempy + } + + + fun mulTransToOutUnsafe(T: Transform, v: Vec2, out: Vec2) { + assert(v !== out) + val px = v.x - T.p.x + val py = v.y - T.p.y + out.x = T.q.c * px + T.q.s * py + out.y = -T.q.s * px + T.q.c * py + } + + + fun mul(A: Transform, B: Transform): Transform { + val C = Transform() + Rot.mulUnsafe(A.q, B.q, C.q) + Rot.mulToOutUnsafe(A.q, B.p, C.p) + C.p.addLocal(A.p) + return C + } + + + fun mulToOut(A: Transform, B: Transform, out: Transform) { + assert(out !== A) + Rot.mul(A.q, B.q, out.q) + Rot.mulToOut(A.q, B.p, out.p) + out.p.addLocal(A.p) + } + + + fun mulToOutUnsafe(A: Transform, B: Transform, out: Transform) { + assert(out !== B) + assert(out !== A) + Rot.mulUnsafe(A.q, B.q, out.q) + Rot.mulToOutUnsafe(A.q, B.p, out.p) + out.p.addLocal(A.p) + } + + fun mulTrans(A: Transform, B: Transform, pool: Vec2 = Vec2()): Transform { + val C = Transform() + Rot.mulTransUnsafe(A.q, B.q, C.q) + pool.set(B.p).subLocal(A.p) + Rot.mulTransUnsafe(A.q, pool, C.p) + return C + } + + + fun mulTransToOut(A: Transform, B: Transform, out: Transform, pool: Vec2 = Vec2()) { + assert(out !== A) + Rot.mulTrans(A.q, B.q, out.q) + pool.set(B.p).subLocal(A.p) + Rot.mulTrans(A.q, pool, out.p) + } + + + fun mulTransToOutUnsafe(A: Transform, B: Transform, out: Transform, pool: Vec2 = Vec2()) { + assert(out !== A) + assert(out !== B) + Rot.mulTransUnsafe(A.q, B.q, out.q) + pool.set(B.p).subLocal(A.p) + Rot.mulTransUnsafe(A.q, pool, out.p) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec2.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec2.kt new file mode 100644 index 0000000..714be6d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec2.kt @@ -0,0 +1,209 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import org.jbox2d.internal.assert +import kotlin.native.concurrent.ThreadLocal + +@ThreadLocal +private val __dummy = Vec2() + +/** + * A 2D column vector + */ +data class Vec2( + var x: Float = 0f, + var y: Float = 0f +) { + /** True if the vector represents a pair of valid, non-infinite floating point numbers. */ + val isValid: Boolean get() = !x.isNaN() && !x.isInfinite() && !y.isNaN() && !y.isInfinite() + + constructor(toCopy: Vec2) : this(toCopy.x, toCopy.y) {} + + /** Zero out this vector. */ + fun setZero() { + x = 0.0f + y = 0.0f + } + + /** Set the vector component-wise. */ + fun set(x: Float, y: Float): Vec2 = this.apply { + this.x = x + this.y = y + } + + inline fun set(x: Number, y: Number): Vec2 = set(x.toFloat(), y.toFloat()) + + /** Set this vector to another vector. */ + fun set(v: Vec2): Vec2 = this.apply { + this.x = v.x + this.y = v.y + } + + /** Return the sum of this vector and another; does not alter either one. */ + fun add(v: Vec2): Vec2 = Vec2(x + v.x, y + v.y) + + + /** Return the difference of this vector and another; does not alter either one. */ + fun sub(v: Vec2): Vec2 = Vec2(x - v.x, y - v.y) + + /** Return this vector multiplied by a scalar; does not alter this vector. */ + fun mul(a: Float): Vec2 = Vec2(x * a, y * a) + + /** Return the negation of this vector; does not alter this vector. */ + fun negate(): Vec2 = Vec2(-x, -y) + + /** Flip the vector and return it - alters this vector. */ + fun negateLocal(): Vec2 = this.apply { + x = -x + y = -y + } + + /** Add another vector to this one and returns result - alters this vector. */ + fun addLocal(v: Vec2): Vec2 = this.apply { + x += v.x + y += v.y + } + + /** Adds values to this vector and returns result - alters this vector. */ + fun addLocal(x: Float, y: Float): Vec2 = this.apply { + this.x += x + this.y += y + } + + inline fun addLocal(x: Number, y: Number): Vec2 = addLocal(x.toFloat(), y.toFloat()) + + /** Subtract another vector from this one and return result - alters this vector. */ + fun subLocal(v: Vec2): Vec2 = this.apply { + x -= v.x + y -= v.y + } + + /** Multiply this vector by a number and return result - alters this vector. */ + fun mulLocal(a: Float): Vec2 = this.apply { + x *= a + y *= a + } + + /** Get the skew vector such that dot(skew_vec, other) == cross(vec, other) */ + fun skew(): Vec2 = Vec2(-y, x) + + /** Get the skew vector such that dot(skew_vec, other) == cross(vec, other) */ + fun skew(out: Vec2) { + out.x = -y + out.y = x + } + + /** Return the length of this vector. */ + fun length(): Float = MathUtils.sqrt(x * x + y * y) + + /** Return the squared length of this vector. */ + fun lengthSquared(): Float = x * x + y * y + + /** Normalize this vector and return the length before normalization. Alters this vector. */ + fun normalize(): Float { + val length = length() + if (length < Settings.EPSILON) { + return 0f + } + + val invLength = 1.0f / length + x *= invLength + y *= invLength + return length + } + + /** Return a new vector that has positive components. */ + fun abs(): Vec2 = Vec2(MathUtils.abs(x), MathUtils.abs(y)) + + fun absLocal() { + x = MathUtils.abs(x) + y = MathUtils.abs(y) + } + + // @Override // annotation omitted for GWT-compatibility + /** Return a copy of this vector. */ + fun clone(): Vec2 = Vec2(x, y) + + override fun toString(): String = "($x,$y)" + + companion object { + internal val dummy get() = __dummy + + fun abs(a: Vec2): Vec2 = Vec2(MathUtils.abs(a.x), MathUtils.abs(a.y)) + + + fun absToOut(a: Vec2, out: Vec2) { + out.x = MathUtils.abs(a.x) + out.y = MathUtils.abs(a.y) + } + + + fun dot(a: Vec2, b: Vec2): Float = a.x * b.x + a.y * b.y + fun cross(a: Vec2, b: Vec2): Float = a.x * b.y - a.y * b.x + fun cross(a: Vec2, s: Float): Vec2 = Vec2(s * a.y, -s * a.x) + + fun crossToOut(a: Vec2, s: Float, out: Vec2) { + val tempy = -s * a.x + out.x = s * a.y + out.y = tempy + } + + fun crossToOutUnsafe(a: Vec2, s: Float, out: Vec2) { + assert(out !== a) + out.x = s * a.y + out.y = -s * a.x + } + + fun cross(s: Float, a: Vec2): Vec2 = Vec2(-s * a.y, s * a.x) + + fun crossToOut(s: Float, a: Vec2, out: Vec2) { + val tempY = s * a.x + out.x = -s * a.y + out.y = tempY + } + + fun crossToOutUnsafe(s: Float, a: Vec2, out: Vec2) { + assert(out !== a) + out.x = -s * a.y + out.y = s * a.x + } + + fun negateToOut(a: Vec2, out: Vec2) { + out.x = -a.x + out.y = -a.y + } + + fun min(a: Vec2, b: Vec2): Vec2 = Vec2(if (a.x < b.x) a.x else b.x, if (a.y < b.y) a.y else b.y) + fun max(a: Vec2, b: Vec2): Vec2 = Vec2(if (a.x > b.x) a.x else b.x, if (a.y > b.y) a.y else b.y) + fun minToOut(a: Vec2, b: Vec2, out: Vec2) { + out.x = if (a.x < b.x) a.x else b.x + out.y = if (a.y < b.y) a.y else b.y + } + fun maxToOut(a: Vec2, b: Vec2, out: Vec2) { + out.x = if (a.x > b.x) a.x else b.x + out.y = if (a.y > b.y) a.y else b.y + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec3.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec3.kt new file mode 100644 index 0000000..6ea9401 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/common/Vec3.kt @@ -0,0 +1,138 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.common + +import org.jbox2d.internal.assert + +/** + * @author Daniel Murphy + */ +data class Vec3( + + var x: Float = 0.toFloat(), + + var y: Float = 0.toFloat(), + + var z: Float = 0.toFloat() +) { + constructor(copy: Vec3) : this(copy.x, copy.y, copy.z) + + fun set(vec: Vec3): Vec3 { + x = vec.x + y = vec.y + z = vec.z + return this + } + + fun set(argX: Float, argY: Float, argZ: Float): Vec3 { + x = argX + y = argY + z = argZ + return this + } + + fun addLocal(argVec: Vec3): Vec3 { + x += argVec.x + y += argVec.y + z += argVec.z + return this + } + + fun add(argVec: Vec3): Vec3 { + return Vec3(x + argVec.x, y + argVec.y, z + argVec.z) + } + + fun subLocal(argVec: Vec3): Vec3 { + x -= argVec.x + y -= argVec.y + z -= argVec.z + return this + } + + fun sub(argVec: Vec3): Vec3 { + return Vec3(x - argVec.x, y - argVec.y, z - argVec.z) + } + + fun mulLocal(argScalar: Float): Vec3 { + x *= argScalar + y *= argScalar + z *= argScalar + return this + } + + fun mul(argScalar: Float): Vec3 { + return Vec3(x * argScalar, y * argScalar, z * argScalar) + } + + fun negate(): Vec3 { + return Vec3(-x, -y, -z) + } + + fun negateLocal(): Vec3 { + x = -x + y = -y + z = -z + return this + } + + fun setZero() { + x = 0f + y = 0f + z = 0f + } + + fun clone(): Vec3 { + return Vec3(this) + } + + override fun toString(): String { + return "($x,$y,$z)" + } + + companion object { + fun dot(a: Vec3, b: Vec3): Float { + return a.x * b.x + a.y * b.y + a.z * b.z + } + + fun cross(a: Vec3, b: Vec3): Vec3 { + return Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x) + } + + fun crossToOut(a: Vec3, b: Vec3, out: Vec3) { + val tempy = a.z * b.x - a.x * b.z + val tempz = a.x * b.y - a.y * b.x + out.x = a.y * b.z - a.z * b.y + out.y = tempy + out.z = tempz + } + + fun crossToOutUnsafe(a: Vec3, b: Vec3, out: Vec3) { + assert(out !== b) + assert(out !== a) + out.x = a.y * b.z - a.z * b.y + out.y = a.z * b.x - a.x * b.z + out.z = a.x * b.y - a.y * b.x + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Body.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Body.kt new file mode 100644 index 0000000..957805f --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Body.kt @@ -0,0 +1,1219 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import korlibs.math.geom.* +import org.jbox2d.collision.shapes.MassData +import org.jbox2d.collision.shapes.Shape +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Sweep +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.contacts.ContactEdge +import org.jbox2d.dynamics.joints.JointEdge +import org.jbox2d.internal.* +import org.jbox2d.userdata.* + +/** + * A rigid body. These are created via World.createBody. + * + * @author Daniel Murphy + */ +class Body(bd: BodyDef, var world: World) : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + fun destroyBody() { + world.destroyBody(this) + } + + val bodyDef = bd + var didReset = true + + class ViewInfo { + var view: Any? = null + var x: Double = 0.0 + var y: Double = 0.0 + var rotation: Angle = 0.degrees + var onStage = false + var firstFrame: Boolean = true + } + + val viewInfo = ViewInfo() + + var _type: BodyType + + + var flags: Int = 0 + + + var islandIndex: Int = 0 + + /** + * The body origin transform. + */ + val xf = Transform() + val transform: Transform get() = xf + + /** + * The previous transform for particle simulation + */ + val xf0 = Transform() + + /** + * The swept motion for CCD + */ + + val sweep = Sweep() + + + val _linearVelocity = Vec2() + + var _angularVelocity = 0f + + + val force = Vec2() + + var torque = 0f + + var prev: Body? = null + + /** Get the next body in the world's body list. */ + + var m_next: Body? = null + + fun getNext() = m_next + + /** Get the list of all fixtures attached to this body. */ + + var m_fixtureList: Fixture? = null + + fun getFixtureList() = m_fixtureList + + + var m_fixtureCount: Int = 0 + + /** Get the list of all joints attached to this body. */ + + var m_jointList: JointEdge? = null + + /** + * Get the list of all contacts attached to this body. + * + * @warning this list changes during the time step and you may miss some collisions if you don't + * use ContactListener. + */ + + var m_contactList: ContactEdge? = null + + fun getContactList() = m_contactList + + /** + * Get the total mass of the body. + * + * @return the mass, usually in kilograms (kg). + */ + + var m_mass: Float = 0.toFloat() + + fun getMass() = m_mass + + + var m_invMass: Float = 0.toFloat() + + // Rotational inertia about the center of mass. + + var m_I: Float = 0.toFloat() + + var m_invI: Float = 0.toFloat() + + /** Get the linear damping of the body. */ + /** Set the linear damping of the body. */ + + var m_linearDamping: Float = 0.toFloat() + /** Get the angular damping of the body. */ + /** Set the angular damping of the body. */ + + var m_angularDamping: Float = 0.toFloat() + /** + * Get the gravity scale of the body. + * + * @return + */ + /** + * Set the gravity scale of the body. + * + * @param gravityScale + */ + + var gravityScale: Float = 0.toFloat() + + var m_sleepTime: Float = 0.toFloat() + + /** Get the user data pointer that was provided in the body definition. */ + /** + * Set the user data. Use this to store your application specific data. + */ + var userData: Any? = null + + private val fixDef = FixtureDef() + + /** + * Get the world body origin position. Do not modify. + * + * @return the world position of the body's origin. + */ + val position: Vec2 + get() = xf.p + + /** + * Get the angle in radians. + * + * @return the current world rotation angle in radians. + */ + var angleRadians: Float + get() = sweep.a + // @TODO: Check this is possible + set(value) { + sweep.a = value + } + + var angle: Angle + get() = angleRadians.radians + set(value) { + angleRadians = value.radians.toFloat() + } + + /** + * Get the angle in degrees. + * + * @return the current world rotation angle in degrees. + */ + var angleDegrees: Float + get() = angle.degrees.toFloat() + set(value) { + angle = value.degrees + } + + + /** + * Get the world position of the center of mass. Do not modify. + */ + val worldCenter: Vec2 + get() = sweep.c + + /** + * Get the local position of the center of mass. Do not modify. + */ + val localCenter: Vec2 + get() = sweep.localCenter + + /** + * Get the linear velocity of the center of mass. Do not modify, instead use + * [.setLinearVelocity]. + * + * @return the linear velocity of the center of mass. + */ + /** + * Set the linear velocity of the center of mass. + * + * @param v the new linear velocity of the center of mass. + */ + var linearVelocity: Vec2 + get() = _linearVelocity + set(v) { + if (_type === BodyType.STATIC) { + return + } + + if (Vec2.dot(v, v) > 0.0f) { + isAwake = true + } + + _linearVelocity.set(v) + } + + var linearVelocityX: Float + get() = linearVelocity.x + set(v) { + linearVelocity = linearVelocity.set(v, linearVelocity.y) + } + + var linearVelocityY: Float + get() = linearVelocity.y + set(v) { + linearVelocity = linearVelocity.set(linearVelocity.x, v) + } + + /** + * Get the angular velocity. + * + * @return the angular velocity in radians/second. + */ + /** + * Set the angular velocity. + * + * @param omega the new angular velocity in radians/second. + */ + var angularVelocity: Float + get() = _angularVelocity + set(w) { + if (_type === BodyType.STATIC) { + return + } + + if (w * w > 0f) { + isAwake = true + } + + _angularVelocity = w + } + + /** + * Get the central rotational inertia of the body. + * + * @return the rotational inertia, usually in kg-m^2. + */ + val inertia: Float + get() = m_I + m_mass * (sweep.localCenter.x * sweep.localCenter.x + sweep.localCenter.y * sweep.localCenter.y) + + private val pmd = MassData() + + /** + * Set the type of this body. This may alter the mass and velocity. + * + * @param type + */ + // Delete the attached contacts. + // Touch the proxies so that new contacts will be created (when appropriate) + var type: BodyType + get() = _type + set(type) { + assert(!world.isLocked) + if (world.isLocked) { + return + } + + if (_type === type) { + return + } + + _type = type + + resetMassData() + + if (_type === BodyType.STATIC) { + _linearVelocity.setZero() + _angularVelocity = 0.0f + sweep.a0 = sweep.a + sweep.c0.set(sweep.c) + synchronizeFixtures() + } + + isAwake = true + + force.setZero() + torque = 0.0f + var ce = m_contactList + while (ce != null) { + val ce0 = ce + ce = ce.next + world.m_contactManager.destroy(ce0.contact!!) + } + m_contactList = null + val broadPhase = world.m_contactManager.m_broadPhase + var f = m_fixtureList + while (f != null) { + val proxyCount = f.m_proxyCount + for (i in 0 until proxyCount) { + broadPhase.touchProxy(f.m_proxies!![i].proxyId) + } + f = f.m_next + } + } + + /** Is this body treated like a bullet for continuous collision detection? */ + /** Should this body be treated like a bullet for continuous collision detection? */ + var isBullet: Boolean + get() = flags and e_bulletFlag == e_bulletFlag + set(flag) = if (flag) { + flags = flags or e_bulletFlag + } else { + flags = flags and e_bulletFlag.inv() + } + + /** + * Is this body allowed to sleep + * + * @return + */ + /** + * You can disable sleeping on this body. If you disable sleeping, the body will be woken. + * + * @param flag + */ + var isSleepingAllowed: Boolean + get() = flags and e_autoSleepFlag == e_autoSleepFlag + set(flag) = if (flag) { + flags = flags or e_autoSleepFlag + } else { + flags = flags and e_autoSleepFlag.inv() + isAwake = true + } + + /** + * Get the sleeping state of this body. + * + * @return true if the body is awake. + */ + /** + * Set the sleep state of the body. A sleeping body has very low CPU cost. + * + * @param flag set to true to put body to sleep, false to wake it. + * @param flag + */ + var isAwake: Boolean + get() = flags and e_awakeFlag == e_awakeFlag + set(flag) { + if (flag) { + if (flags and e_awakeFlag == 0) { + flags = flags or e_awakeFlag + m_sleepTime = 0.0f + } + } else { + flags = flags and e_awakeFlag.inv() + m_sleepTime = 0.0f + _linearVelocity.setZero() + _angularVelocity = 0.0f + force.setZero() + torque = 0.0f + } + } + + /** + * Get the active state of the body. + * + * @return + */ + /** + * Set the active state of the body. An inactive body is not simulated and cannot be collided with + * or woken up. If you pass a flag of true, all fixtures will be added to the broad-phase. If you + * pass a flag of false, all fixtures will be removed from the broad-phase and all contacts will + * be destroyed. Fixtures and joints are otherwise unaffected. You may continue to create/destroy + * fixtures and joints on inactive bodies. Fixtures on an inactive body are implicitly inactive + * and will not participate in collisions, ray-casts, or queries. Joints connected to an inactive + * body are implicitly inactive. An inactive body is still owned by a World object and remains in + * the body list. + * + * @param flag + */ + // Create all proxies. + // Contacts are created the next time step. + // Destroy all proxies. + // Destroy the attached contacts. + var isActive: Boolean + get() = flags and e_activeFlag == e_activeFlag + set(flag) { + assert(!world.isLocked) + + if (flag == isActive) { + return + } + + if (flag) { + flags = flags or e_activeFlag + val broadPhase = world.m_contactManager.m_broadPhase + var f = m_fixtureList + while (f != null) { + f.createProxies(broadPhase, xf) + f = f.m_next + } + } else { + flags = flags and e_activeFlag.inv() + val broadPhase = world.m_contactManager.m_broadPhase + var f = m_fixtureList + while (f != null) { + f.destroyProxies(broadPhase) + f = f.m_next + } + var ce = m_contactList + while (ce != null) { + val ce0 = ce + ce = ce.next + world.m_contactManager.destroy(ce0.contact!!) + } + m_contactList = null + } + } + + /** + * Does this body have fixed rotation? + * + * @return + */ + /** + * Set this body to have fixed rotation. This causes the mass to be reset. + * + * @param flag + */ + var isFixedRotation: Boolean + get() = flags and e_fixedRotationFlag == e_fixedRotationFlag + set(flag) { + if (flag) { + flags = flags or e_fixedRotationFlag + } else { + flags = flags and e_fixedRotationFlag.inv() + } + + resetMassData() + } + + // djm pooling + private val pxf = Transform() + + + init { + assert(bd.position.isValid) + assert(bd.linearVelocity.isValid) + assert(bd.gravityScale >= 0.0f) + assert(bd.angularDamping >= 0.0f) + assert(bd.linearDamping >= 0.0f) + + flags = 0 + + if (bd.bullet) { + flags = flags or e_bulletFlag + } + if (bd.fixedRotation) { + flags = flags or e_fixedRotationFlag + } + if (bd.allowSleep) { + flags = flags or e_autoSleepFlag + } + if (bd.awake) { + flags = flags or e_awakeFlag + } + if (bd.active) { + flags = flags or e_activeFlag + } + + xf.p.set(bd.position) + xf.q.setRadians(bd.angleRadians) + + sweep.localCenter.setZero() + sweep.c0.set(xf.p) + sweep.c.set(xf.p) + sweep.a0 = bd.angleRadians + sweep.a = bd.angleRadians + sweep.alpha0 = 0.0f + + m_jointList = null + m_contactList = null + prev = null + m_next = null + + _linearVelocity.set(bd.linearVelocity) + _angularVelocity = bd.angularVelocity + + m_linearDamping = bd.linearDamping + m_angularDamping = bd.angularDamping + gravityScale = bd.gravityScale + + force.setZero() + torque = 0.0f + + m_sleepTime = 0.0f + + _type = bd.type + + if (_type === BodyType.DYNAMIC) { + m_mass = 1f + m_invMass = 1f + } else { + m_mass = 0f + m_invMass = 0f + } + + m_I = 0.0f + m_invI = 0.0f + + userData = bd.userData + + m_fixtureList = null + m_fixtureCount = 0 + } + + /** + * Creates a fixture and attach it to this body. Use this function if you need to set some fixture + * parameters, like friction. Otherwise you can create the fixture directly from a shape. If the + * density is non-zero, this function automatically updates the mass of the body. Contacts are not + * created until the next time step. + * + * @param def the fixture definition. + * @warning This function is locked during callbacks. + */ + fun createFixture(def: FixtureDef): Fixture? { + assert(!world.isLocked) + + if (world.isLocked) { + return null + } + + val fixture = Fixture() + fixture.create(this, def) + + if (flags and e_activeFlag == e_activeFlag) { + val broadPhase = world.m_contactManager.m_broadPhase + fixture.createProxies(broadPhase, xf) + } + + fixture.m_next = m_fixtureList + m_fixtureList = fixture + ++m_fixtureCount + + fixture.m_body = this + + // Adjust mass properties if needed. + if (fixture.m_density > 0.0f) { + resetMassData() + } + + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + world.m_flags = world.m_flags or World.NEW_FIXTURE + + return fixture + } + + /** + * Creates a fixture from a shape and attach it to this body. This is a convenience function. Use + * FixtureDef if you need to set parameters like friction, restitution, user data, or filtering. + * If the density is non-zero, this function automatically updates the mass of the body. + * + * @param shape the shape to be cloned. + * @param density the shape density (set to zero for static bodies). + * @warning This function is locked during callbacks. + */ + fun createFixture(shape: Shape, density: Float): Fixture? { + fixDef.shape = shape + fixDef.density = density + + return createFixture(fixDef) + } + + /** + * Destroy a fixture. This removes the fixture from the broad-phase and destroys all contacts + * associated with this fixture. This will automatically adjust the mass of the body if the body + * is dynamic and the fixture has positive density. All fixtures attached to a body are implicitly + * destroyed when the body is destroyed. + * + * @param fixture the fixture to be removed. + * @warning This function is locked during callbacks. + */ + fun destroyFixture(fixture: Fixture?) { + var fixture = fixture + assert(!world.isLocked) + if (world.isLocked) { + return + } + + assert(fixture!!.m_body === this) + + // Remove the fixture from this body's singly linked list. + assert(m_fixtureCount > 0) + var node = m_fixtureList + var last: Fixture? = null // java change + var found = false + while (node != null) { + if (node === fixture) { + node = fixture.m_next + found = true + break + } + last = node + node = node.m_next + } + + // You tried to remove a shape that is not attached to this body. + assert(found) + + // java change, remove it from the list + if (last == null) { + m_fixtureList = fixture!!.m_next + } else { + last.m_next = fixture!!.m_next + } + + // Destroy any contacts associated with the fixture. + var edge = m_contactList + while (edge != null) { + val c = edge.contact + edge = edge.next + + val fixtureA = c!!.getFixtureA() + val fixtureB = c.getFixtureB() + + if (fixture === fixtureA || fixture === fixtureB) { + // This destroys the contact and removes it from + // this body's contact list. + world.m_contactManager.destroy(c) + } + } + + if (flags and e_activeFlag == e_activeFlag) { + val broadPhase = world.m_contactManager.m_broadPhase + fixture.destroyProxies(broadPhase) + } + + fixture.destroy() + fixture.m_body = null + fixture.m_next = null + fixture = null + + --m_fixtureCount + + // Reset the mass data. + resetMassData() + } + + /** + * Set the position of the body's origin and rotation. This breaks any contacts and wakes the + * other bodies. Manipulating a body's transform may cause non-physical behavior. Note: contacts + * are updated on the next call to World.step(). + * + * @param position the world position of the body's local origin. + * @param angleRadians the world rotation in radians. + */ + fun setTransformRadians(position: Vec2, angleRadians: Float) { + require(position.isValid) { "Invalid position $position" } + require(angleRadians.isFinite()) { "Invalid angleRadians $angleRadians" } + + assert(!world.isLocked) + if (world.isLocked) { + return + } + + xf.q.setRadians(angleRadians) + xf.p.set(position) + + // m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter); + Transform.mulToOutUnsafe(xf, sweep.localCenter, sweep.c) + sweep.a = angleRadians + + sweep.c0.set(sweep.c) + sweep.a0 = sweep.a + + val broadPhase = world.m_contactManager.m_broadPhase + var f = m_fixtureList + while (f != null) { + f.synchronize(broadPhase, xf, xf) + f = f.m_next + } + } + + /** + * Set the position of the body's origin and rotation. This breaks any contacts and wakes the + * other bodies. Manipulating a body's transform may cause non-physical behavior. Note: contacts + * are updated on the next call to World.step(). + * + * @param position the world position of the body's local origin. + * @param angleDegrees the world rotation in degrees. + */ + fun setTransformDegrees(position: Vec2, angleDegrees: Float) = setTransformRadians(position, angleDegrees * MathUtils.DEG2RAD) + + /** + * Set the position of the body's origin and rotation. This breaks any contacts and wakes the + * other bodies. Manipulating a body's transform may cause non-physical behavior. Note: contacts + * are updated on the next call to World.step(). + * + * @param position the world position of the body's local origin. + * @param angle the world rotation. + */ + fun setTransform(position: Vec2, angle: Angle) = setTransformRadians(position, angle.radians.toFloat()) + + /** + * Apply a force at a world point. If the force is not applied at the center of mass, it will + * generate a torque and affect the angular velocity. This wakes up the body. + * + * @param force the world force vector, usually in Newtons (N). + * @param point the world position of the point of application. + */ + fun applyForce(force: Vec2, point: Vec2) { + require(force.isValid) { "Invalid force $force" } + require(point.isValid) { "Invalid point $point" } + if (_type !== BodyType.DYNAMIC) { + return + } + + if (!isAwake) { + isAwake = true + } + + // m_force.addLocal(force); + // Vec2 temp = tltemp.get(); + // temp.set(point).subLocal(m_sweep.c); + // m_torque += Vec2.cross(temp, force); + + this.force.x += force.x + this.force.y += force.y + + torque += (point.x - sweep.c.x) * force.y - (point.y - sweep.c.y) * force.x + } + + /** + * Apply a force to the center of mass. This wakes up the body. + * + * @param force the world force vector, usually in Newtons (N). + */ + fun applyForceToCenter(force: Vec2) { + require(force.isValid) { "Invalid force $force" } + if (_type !== BodyType.DYNAMIC) { + return + } + + if (!isAwake) { + isAwake = true + } + + this.force.x += force.x + this.force.y += force.y + } + + /** + * Apply a torque. This affects the angular velocity without affecting the linear velocity of the + * center of mass. This wakes up the body. + * + * @param torque about the z-axis (out of the screen), usually in N-m. + */ + fun applyTorque(torque: Float) { + require(torque.isFinite()) { "Invalid torque $torque" } + if (_type !== BodyType.DYNAMIC) { + return + } + + if (!isAwake) { + isAwake = true + } + + this.torque += torque + } + + /** + * Apply an impulse at a point. This immediately modifies the velocity. It also modifies the + * angular velocity if the point of application is not at the center of mass. This wakes up the + * body if 'wake' is set to true. If the body is sleeping and 'wake' is false, then there is no + * effect. + * + * @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + * @param point the world position of the point of application. + * @param wake also wake up the body + */ + fun applyLinearImpulse(impulse: Vec2, point: Vec2, wake: Boolean) { + require(point.isValid) { "Invalid point $point" } + require(impulse.isValid) { "Invalid impulse $impulse" } + if (_type !== BodyType.DYNAMIC) { + return + } + + if (!isAwake) { + if (wake) { + isAwake = true + } else { + return + } + } + + _linearVelocity.x += impulse.x * m_invMass + _linearVelocity.y += impulse.y * m_invMass + + _angularVelocity += m_invI * ((point.x - sweep.c.x) * impulse.y - (point.y - sweep.c.y) * impulse.x) + } + + /** + * Apply an angular impulse. + * + * @param impulse the angular impulse in units of kg*m*m/s + */ + fun applyAngularImpulse(impulse: Float) { + require(impulse.isFinite()) { "Invalid impulse $impulse" } + if (_type !== BodyType.DYNAMIC) { + return + } + + if (!isAwake) { + isAwake = true + } + _angularVelocity += m_invI * impulse + } + + /** + * Get the mass data of the body. The rotational inertia is relative to the center of mass. + * + * @return a struct containing the mass, inertia and center of the body. + */ + fun getMassData(data: MassData) { + // data.mass = m_mass; + // data.I = m_I + m_mass * Vec2.dot(m_sweep.localCenter, m_sweep.localCenter); + // data.center.set(m_sweep.localCenter); + + data.mass = m_mass + data.I = m_I + m_mass * (sweep.localCenter.x * sweep.localCenter.x + sweep.localCenter.y * sweep.localCenter.y) + data.center.x = sweep.localCenter.x + data.center.y = sweep.localCenter.y + } + + /** + * Set the mass properties to override the mass properties of the fixtures. Note that this changes + * the center of mass position. Note that creating or destroying fixtures can also alter the mass. + * This function has no effect if the body isn't dynamic. + * + * @param massData the mass properties. + */ + fun setMassData(massData: MassData) { + // TODO_ERIN adjust linear velocity and torque to account for movement of center. + assert(!world.isLocked) + if (world.isLocked) { + return + } + + if (_type !== BodyType.DYNAMIC) { + return + } + + m_invMass = 0.0f + m_I = 0.0f + m_invI = 0.0f + + m_mass = massData.mass + if (m_mass <= 0.0f) { + m_mass = 1f + } + + m_invMass = 1.0f / m_mass + + if (massData.I > 0.0f && flags and e_fixedRotationFlag == 0) { + m_I = massData.I - m_mass * Vec2.dot(massData.center, massData.center) + assert(m_I > 0.0f) + m_invI = 1.0f / m_I + } + + val oldCenter = world.pool.popVec2() + // Move center of mass. + oldCenter.set(sweep.c) + sweep.localCenter.set(massData.center) + // m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter); + Transform.mulToOutUnsafe(xf, sweep.localCenter, sweep.c0) + sweep.c.set(sweep.c0) + + // Update center of mass velocity. + // m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter); + val temp = world.pool.popVec2() + temp.set(sweep.c).subLocal(oldCenter) + Vec2.crossToOut(_angularVelocity, temp, temp) + _linearVelocity.addLocal(temp) + + world.pool.pushVec2(2) + } + + /** + * This resets the mass properties to the sum of the mass properties of the fixtures. This + * normally does not need to be called unless you called setMassData to override the mass and you + * later want to reset the mass. + */ + fun resetMassData() { + // Compute mass data from shapes. Each shape has its own density. + m_mass = 0.0f + m_invMass = 0.0f + m_I = 0.0f + m_invI = 0.0f + sweep.localCenter.setZero() + + // Static and kinematic bodies have zero mass. + if (_type === BodyType.STATIC || _type === BodyType.KINEMATIC) { + // m_sweep.c0 = m_sweep.c = m_xf.position; + sweep.c0.set(xf.p) + sweep.c.set(xf.p) + sweep.a0 = sweep.a + return + } + + assert(_type === BodyType.DYNAMIC) + + // Accumulate mass over all fixtures. + val localCenter = world.pool.popVec2() + localCenter.setZero() + val temp = world.pool.popVec2() + val massData = pmd + var f = m_fixtureList + while (f != null) { + if (f.m_density == 0.0f) { + f = f.m_next + continue + } + f.getMassData(massData) + m_mass += massData.mass + // center += massData.mass * massData.center; + temp.set(massData.center).mulLocal(massData.mass) + localCenter.addLocal(temp) + m_I += massData.I + f = f.m_next + } + + // Compute center of mass. + if (m_mass > 0.0f) { + m_invMass = 1.0f / m_mass + localCenter.mulLocal(m_invMass) + } else { + // Force all dynamic bodies to have a positive mass. + m_mass = 1.0f + m_invMass = 1.0f + } + + if (m_I > 0.0f && flags and e_fixedRotationFlag == 0) { + // Center the inertia about the center of mass. + m_I -= m_mass * Vec2.dot(localCenter, localCenter) + assert(m_I > 0.0f) + m_invI = 1.0f / m_I + } else { + m_I = 0.0f + m_invI = 0.0f + } + + val oldCenter = world.pool.popVec2() + // Move center of mass. + oldCenter.set(sweep.c) + sweep.localCenter.set(localCenter) + // m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter); + Transform.mulToOutUnsafe(xf, sweep.localCenter, sweep.c0) + sweep.c.set(sweep.c0) + + // Update center of mass velocity. + // m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter); + temp.set(sweep.c).subLocal(oldCenter) + + val temp2 = oldCenter + Vec2.crossToOutUnsafe(_angularVelocity, temp, temp2) + _linearVelocity.addLocal(temp2) + + world.pool.pushVec2(3) + } + + /** + * Get the world coordinates of a point given the local coordinates. + * + * @param localPoint a point on the body measured relative the the body's origin. + * @return the same point expressed in world coordinates. + */ + fun getWorldPoint(localPoint: Vec2): Vec2 { + val v = Vec2() + getWorldPointToOut(localPoint, v) + return v + } + + fun getWorldPointToOut(localPoint: Vec2, out: Vec2) { + Transform.mulToOut(xf, localPoint, out) + } + + /** + * Get the world coordinates of a vector given the local coordinates. + * + * @param localVector a vector fixed in the body. + * @return the same vector expressed in world coordinates. + */ + fun getWorldVector(localVector: Vec2): Vec2 { + val out = Vec2() + getWorldVectorToOut(localVector, out) + return out + } + + fun getWorldVectorToOut(localVector: Vec2, out: Vec2) { + Rot.mulToOut(xf.q, localVector, out) + } + + fun getWorldVectorToOutUnsafe(localVector: Vec2, out: Vec2) { + Rot.mulToOutUnsafe(xf.q, localVector, out) + } + + /** + * Gets a local point relative to the body's origin given a world point. + * + * @param a point in world coordinates. + * @return the corresponding local point relative to the body's origin. + */ + fun getLocalPoint(worldPoint: Vec2): Vec2 { + val out = Vec2() + getLocalPointToOut(worldPoint, out) + return out + } + + fun getLocalPointToOut(worldPoint: Vec2, out: Vec2) { + Transform.mulTransToOut(xf, worldPoint, out) + } + + /** + * Gets a local vector given a world vector. + * + * @param a vector in world coordinates. + * @return the corresponding local vector. + */ + fun getLocalVector(worldVector: Vec2): Vec2 { + val out = Vec2() + getLocalVectorToOut(worldVector, out) + return out + } + + fun getLocalVectorToOut(worldVector: Vec2, out: Vec2) { + Rot.mulTrans(xf.q, worldVector, out) + } + + fun getLocalVectorToOutUnsafe(worldVector: Vec2, out: Vec2) { + Rot.mulTransUnsafe(xf.q, worldVector, out) + } + + /** + * Get the world linear velocity of a world point attached to this body. + * + * @param a point in world coordinates. + * @return the world velocity of a point. + */ + fun getLinearVelocityFromWorldPoint(worldPoint: Vec2): Vec2 { + val out = Vec2() + getLinearVelocityFromWorldPointToOut(worldPoint, out) + return out + } + + fun getLinearVelocityFromWorldPointToOut(worldPoint: Vec2, out: Vec2) { + val tempX = worldPoint.x - sweep.c.x + val tempY = worldPoint.y - sweep.c.y + out.x = -_angularVelocity * tempY + _linearVelocity.x + out.y = _angularVelocity * tempX + _linearVelocity.y + } + + /** + * Get the world velocity of a local point. + * + * @param a point in local coordinates. + * @return the world velocity of a point. + */ + fun getLinearVelocityFromLocalPoint(localPoint: Vec2): Vec2 { + val out = Vec2() + getLinearVelocityFromLocalPointToOut(localPoint, out) + return out + } + + fun getLinearVelocityFromLocalPointToOut(localPoint: Vec2, out: Vec2) { + getWorldPointToOut(localPoint, out) + getLinearVelocityFromWorldPointToOut(out, out) + } + + fun synchronizeFixtures() { + val xf1 = pxf + // xf1.position = m_sweep.c0 - Mul(xf1.R, m_sweep.localCenter); + + // xf1.q.set(m_sweep.a0); + // Rot.mulToOutUnsafe(xf1.q, m_sweep.localCenter, xf1.p); + // xf1.p.mulLocal(-1).addLocal(m_sweep.c0); + // inlined: + xf1.q.s = MathUtils.sin(sweep.a0) + xf1.q.c = MathUtils.cos(sweep.a0) + xf1.p.x = sweep.c0.x - xf1.q.c * sweep.localCenter.x + xf1.q.s * sweep.localCenter.y + xf1.p.y = sweep.c0.y - xf1.q.s * sweep.localCenter.x - xf1.q.c * sweep.localCenter.y + // end inline + + var f = m_fixtureList + while (f != null) { + f.synchronize(world.m_contactManager.m_broadPhase, xf1, xf) + f = f.m_next + } + } + + fun synchronizeTransform() { + // m_xf.q.set(m_sweep.a); + // + // // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter); + // Rot.mulToOutUnsafe(m_xf.q, m_sweep.localCenter, m_xf.p); + // m_xf.p.mulLocal(-1).addLocal(m_sweep.c); + // + xf.q.s = MathUtils.sin(sweep.a) + xf.q.c = MathUtils.cos(sweep.a) + val q = xf.q + val v = sweep.localCenter + xf.p.x = sweep.c.x - q.c * v.x + q.s * v.y + xf.p.y = sweep.c.y - q.s * v.x - q.c * v.y + } + + /** + * This is used to prevent connected bodies from colliding. It may lie, depending on the + * collideConnected flag. + * + * @param other + * @return + */ + fun shouldCollide(other: Body): Boolean { + // At least one body should be dynamic. + if (_type !== BodyType.DYNAMIC && other._type !== BodyType.DYNAMIC) { + return false + } + + // Does a joint prevent collision? + var jn = m_jointList + while (jn != null) { + if (jn.other === other) { + if (!jn.joint!!.getCollideConnected()) { + return false + } + } + jn = jn.next + } + + return true + } + + fun advance(t: Float) { + // Advance to the new safe time. This doesn't sync the broad-phase. + sweep.advance(t) + sweep.c.set(sweep.c0) + sweep.a = sweep.a0 + xf.q.setRadians(sweep.a) + // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter); + Rot.mulToOutUnsafe(xf.q, sweep.localCenter, xf.p) + xf.p.mulLocal(-1f).addLocal(sweep.c) + } + + companion object { + + val e_islandFlag = 0x0001 + + val e_awakeFlag = 0x0002 + + val e_autoSleepFlag = 0x0004 + + val e_bulletFlag = 0x0008 + + val e_fixedRotationFlag = 0x0010 + + val e_activeFlag = 0x0020 + + val e_toiFlag = 0x0040 + } +} + diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyDef.kt new file mode 100644 index 0000000..ea5e82b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyDef.kt @@ -0,0 +1,133 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.userdata.Box2dTypedUserData + +/** + * A body definition holds all the data needed to construct a rigid body. You can safely re-use body + * definitions. Shapes are added to a body after construction. + * + * @author daniel + */ +data class BodyDef( + /** + * The body type: static, kinematic, or dynamic. Note: if a dynamic body would have zero mass, the + * mass is set to one. + */ + var type: BodyType = BodyType.STATIC, + + /** + * Use this to store application specific body data. + */ + var userData: Any? = null, + + /** + * The world position of the body. Avoid creating bodies at the origin since this can lead to many + * overlapping shapes. + */ + var position: Vec2 = Vec2(), + + /** + * The world angle of the body in radians. + */ + var angleRadians: Float = 0f, + + /** + * The linear velocity of the body in world co-ordinates. + */ + var linearVelocity: Vec2 = Vec2(), + + /** + * The angular velocity of the body. + */ + var angularVelocity: Float = 0f, + + /** + * Linear damping is use to reduce the linear velocity. The damping parameter can be larger than + * 1.0f but the damping effect becomes sensitive to the time step when the damping parameter is + * large. + */ + var linearDamping: Float = 0f, + + /** + * Angular damping is use to reduce the angular velocity. The damping parameter can be larger than + * 1.0f but the damping effect becomes sensitive to the time step when the damping parameter is + * large. + */ + var angularDamping: Float = 0f, + + /** + * Set this flag to false if this body should never fall asleep. Note that this increases CPU + * usage. + */ + var allowSleep: Boolean = true, + + /** + * Is this body initially sleeping? + */ + var awake: Boolean = true, + + /** + * Should this body be prevented from rotating? Useful for characters. + */ + var fixedRotation: Boolean = false, + + /** + * Is this a fast moving body that should be prevented from tunneling through other moving bodies? + * Note that all bodies are prevented from tunneling through kinematic and static bodies. This + * setting is only considered on dynamic bodies. + * + * @warning You should use this flag sparingly since it increases processing time. + */ + var bullet: Boolean = false, + + /** + * Does this body start out active? + */ + var active: Boolean = true, + + /** + * Experimental: scales the inertia tensor. + */ + var gravityScale: Float = 1f +) : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + /** + * The world angle of the body in degrees. + */ + var angleDegrees: Float + set(value) { angleRadians = value * MathUtils.DEG2RAD } + get() = angleRadians * MathUtils.RAD2DEG + + /** + * The world angle of the body. + */ + var angle: Angle + set(value) { angleRadians = value.radians.toFloat() } + get() = angleRadians.radians +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyExt.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyExt.kt new file mode 100644 index 0000000..0b49b71 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyExt.kt @@ -0,0 +1,9 @@ +package org.jbox2d.dynamics + +inline fun Body.forEachFixture(callback: (fixture: Fixture) -> Unit) { + var node = m_fixtureList + while (node != null) { + callback(node) + node = node.m_next + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyType.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyType.kt new file mode 100644 index 0000000..3719865 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/BodyType.kt @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 3:59:59 AM Jul 7, 2010 + */ +package org.jbox2d.dynamics + +// updated to rev 100 + +/** + * The body type. + * static: zero mass, zero velocity, may be manually moved + * kinematic: zero mass, non-zero velocity set by user, moved by solver + * dynamic: positive mass, non-zero velocity determined by forces, moved by solver + * + * @author daniel + */ +enum class BodyType { + STATIC, KINEMATIC, DYNAMIC; + + companion object { + val BY_NAME = values().associateBy { it.name.toUpperCase() } + operator fun get(name: String): BodyType = BY_NAME[name.toUpperCase()] ?: STATIC + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/ContactManager.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/ContactManager.kt new file mode 100644 index 0000000..1abea76 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/ContactManager.kt @@ -0,0 +1,275 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.callbacks.ContactFilter +import org.jbox2d.callbacks.ContactListener +import org.jbox2d.callbacks.PairCallback +import org.jbox2d.collision.broadphase.BroadPhase +import org.jbox2d.dynamics.contacts.Contact + +/** + * Delegate of World. + * + * @author Daniel Murphy + */ +class ContactManager(private val pool: World, var m_broadPhase: BroadPhase) : PairCallback { + var m_contactList: Contact? = null + var m_contactCount: Int = 0 + var m_contactFilter: ContactFilter = ContactFilter() + var m_contactListener: ContactListener? = null + + /** + * Broad-phase callback. + * + * @param proxyUserDataA + * @param proxyUserDataB + */ + override fun addPair(proxyUserDataA: Any?, proxyUserDataB: Any?) { + val proxyA = proxyUserDataA as FixtureProxy? + val proxyB = proxyUserDataB as FixtureProxy? + + var fixtureA = proxyA!!.fixture + var fixtureB = proxyB!!.fixture + + var indexA = proxyA.childIndex + var indexB = proxyB.childIndex + + var bodyA = fixtureA!!.getBody() + var bodyB = fixtureB!!.getBody() + + // Are the fixtures on the same body? + if (bodyA == bodyB) { + return + } + + // TODO_ERIN use a hash table to remove a potential bottleneck when both + // bodies have a lot of contacts. + // Does a contact already exist? + var edge = bodyB!!.getContactList() + while (edge != null) { + if (edge.other == bodyA) { + val fA = edge.contact!!.getFixtureA() + val fB = edge.contact!!.getFixtureB() + val iA = edge.contact!!.getChildIndexA() + val iB = edge.contact!!.getChildIndexB() + + if (fA == fixtureA && iA == indexA && fB == fixtureB && iB == indexB) { + // A contact already exists. + return + } + + if (fA == fixtureB && iA == indexB && fB == fixtureA && iB == indexA) { + // A contact already exists. + return + } + } + + edge = edge.next + } + + // Does a joint override collision? is at least one body dynamic? + if (!bodyB.shouldCollide(bodyA!!)) { + return + } + + // Check user filtering. + if (m_contactFilter != null && !m_contactFilter!!.shouldCollide(fixtureA, fixtureB)) { + return + } + + // Call the factory. + val c = pool.popContact(fixtureA, indexA, fixtureB, indexB) ?: return + + // Contact creation may swap fixtures. + fixtureA = c.getFixtureA() + fixtureB = c.getFixtureB() + indexA = c.getChildIndexA() + indexB = c.getChildIndexB() + bodyA = fixtureA!!.getBody() + bodyB = fixtureB!!.getBody() + + // Insert into the world. + c.m_prev = null + c.m_next = m_contactList + if (m_contactList != null) { + m_contactList!!.m_prev = c + } + m_contactList = c + + // Connect to island graph. + + // Connect to body A + c.m_nodeA.contact = c + c.m_nodeA.other = bodyB + + c.m_nodeA.prev = null + c.m_nodeA.next = bodyA!!.m_contactList + if (bodyA.m_contactList != null) { + bodyA.m_contactList!!.prev = c.m_nodeA + } + bodyA.m_contactList = c.m_nodeA + + // Connect to body B + c.m_nodeB.contact = c + c.m_nodeB.other = bodyA + + c.m_nodeB.prev = null + c.m_nodeB.next = bodyB!!.m_contactList + if (bodyB.m_contactList != null) { + bodyB.m_contactList!!.prev = c.m_nodeB + } + bodyB.m_contactList = c.m_nodeB + + // wake up the bodies + if (!fixtureA.isSensor && !fixtureB.isSensor) { + bodyA.isAwake = true + bodyB.isAwake = true + } + + ++m_contactCount + } + + fun findNewContacts() { + m_broadPhase.updatePairs(this) + } + + fun destroy(c: Contact) { + val fixtureA = c.getFixtureA() + val fixtureB = c.getFixtureB() + val bodyA = fixtureA!!.getBody() + val bodyB = fixtureB!!.getBody() + + if (m_contactListener != null && c.isTouching) { + m_contactListener!!.endContact(c) + } + + // Remove from the world. + if (c.m_prev != null) { + c.m_prev!!.m_next = c.m_next + } + + if (c.m_next != null) { + c.m_next!!.m_prev = c.m_prev + } + + if (c === m_contactList) { + m_contactList = c.m_next + } + + // Remove from body 1 + if (c.m_nodeA.prev != null) { + c.m_nodeA.prev!!.next = c.m_nodeA.next + } + + if (c.m_nodeA.next != null) { + c.m_nodeA.next!!.prev = c.m_nodeA.prev + } + + if (c.m_nodeA == bodyA!!.m_contactList) { + bodyA.m_contactList = c.m_nodeA.next + } + + // Remove from body 2 + if (c.m_nodeB.prev != null) { + c.m_nodeB.prev!!.next = c.m_nodeB.next + } + + if (c.m_nodeB.next != null) { + c.m_nodeB.next!!.prev = c.m_nodeB.prev + } + + if (c.m_nodeB == bodyB!!.m_contactList) { + bodyB.m_contactList = c.m_nodeB.next + } + + // Call the factory. + pool.pushContact(c) + --m_contactCount + } + + /** + * This is the top level collision call for the time step. Here all the narrow phase collision is + * processed for the world contact list. + */ + fun collide() { + // Update awake contacts. + var c = m_contactList + while (c != null) { + val fixtureA = c.getFixtureA() + val fixtureB = c.getFixtureB() + val indexA = c.getChildIndexA() + val indexB = c.getChildIndexB() + val bodyA = fixtureA!!.getBody() + val bodyB = fixtureB!!.getBody() + + // is this contact flagged for filtering? + if (c.m_flags and Contact.FILTER_FLAG == Contact.FILTER_FLAG) { + // Should these bodies collide? + if (!bodyB!!.shouldCollide(bodyA!!)) { + val cNuke = c + c = cNuke.getNext() + destroy(cNuke) + continue + } + + // Check user filtering. + if (m_contactFilter != null && !m_contactFilter!!.shouldCollide(fixtureA, fixtureB)) { + val cNuke = c + c = cNuke.getNext() + destroy(cNuke) + continue + } + + // Clear the filtering flag. + c.m_flags = c.m_flags and Contact.FILTER_FLAG.inv() + } + + val activeA = bodyA!!.isAwake && bodyA._type !== BodyType.STATIC + val activeB = bodyB!!.isAwake && bodyB._type !== BodyType.STATIC + + // At least one body must be awake and it must be dynamic or kinematic. + if (!activeA && !activeB) { + c = c.getNext() + continue + } + + val proxyIdA = fixtureA.m_proxies!![indexA].proxyId + val proxyIdB = fixtureB.m_proxies!![indexB].proxyId + val overlap = m_broadPhase.testOverlap(proxyIdA, proxyIdB) + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (!overlap) { + val cNuke = c + c = cNuke.getNext() + destroy(cNuke) + continue + } + + // The contact persists. + c.update(m_contactListener) + c = c.getNext() + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Filter.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Filter.kt new file mode 100644 index 0000000..cceff5e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Filter.kt @@ -0,0 +1,59 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +// updated to rev 100 +/** + * This holds contact filtering data. + * + * @author daniel + */ +class Filter { + /** + * The collision category bits. Normally you would just set one bit. + */ + + var categoryBits: Int = 0x0001 + + /** + * The collision mask bits. This states the categories that this + * shape would accept for collision. + */ + + var maskBits: Int = 0xFFFF + + /** + * Collision groups allow a certain group of objects to never collide (negative) + * or always collide (positive). Zero means no collision group. Non-zero group + * filtering always wins against the mask bits. + */ + + var groupIndex: Int = 0 + + fun set(argOther: Filter) { + categoryBits = argOther.categoryBits + maskBits = argOther.maskBits + groupIndex = argOther.groupIndex + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Fixture.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Fixture.kt new file mode 100644 index 0000000..55c6159 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Fixture.kt @@ -0,0 +1,401 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.collision.* +import org.jbox2d.collision.broadphase.* +import org.jbox2d.collision.shapes.* +import org.jbox2d.common.* +import org.jbox2d.internal.* +import org.jbox2d.userdata.* + +/** + * A fixture is used to attach a shape to a body for collision detection. A fixture inherits its + * transform from its parent. Fixtures hold additional non-geometric data such as friction, + * collision filters, etc. Fixtures are created via Body::CreateFixture. + * + * @warning you cannot reuse fixtures. + * + * @author daniel + */ +class Fixture : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + + var m_density: Float = 0.toFloat() + + /** + * Get the next fixture in the parent body's fixture list. + * + * @return the next shape. + * @return + */ + + var m_next: Fixture? = null + + fun getNext() = m_next + + /** + * Get the parent body of this fixture. This is NULL if the fixture is not attached. + * + * @return the parent body. + * @return + */ + var m_body: Body? = null + + fun getBody() = m_body + + /** + * Get the child shape. You can modify the child shape, however you should not change the number + * of vertices because this will crash some collision caching mechanisms. + * + * @return + */ + var m_shape: Shape? = null + + fun getShape() = m_shape + + /** + * Get the coefficient of friction. + * + * @return + */ + /** + * Set the coefficient of friction. This will _not_ change the friction of existing contacts. + * + * @param friction + */ + var m_friction: Float = 0.toFloat() + /** + * Get the coefficient of restitution. + * + * @return + */ + /** + * Set the coefficient of restitution. This will _not_ change the restitution of existing + * contacts. + * + * @param restitution + */ + var m_restitution: Float = 0.toFloat() + + + var m_proxies: Array? = null + + var m_proxyCount: Int = 0 + + val m_filter: Filter = Filter() + + + var m_isSensor: Boolean = false + + /** + * Get the user data that was assigned in the fixture definition. Use this to store your + * application specific data. + * + * @return + */ + /** + * Set the user data. Use this to store your application specific data. + * + * @param data + */ + var userData: Any? = null + + /** + * Get the type of the child shape. You can use this to down cast to the concrete shape. + * + * @return the shape type. + */ + val type: ShapeType + get() = m_shape!!.getType() + + /** + * Is this fixture a sensor (non-solid)? + * + * @return the true if the shape is a sensor. + * @return + */ + /** + * Set if this fixture is a sensor. + * + * @param sensor + */ + var isSensor: Boolean + get() = m_isSensor + set(sensor) { + if (sensor != m_isSensor) { + m_body!!.isAwake = true + m_isSensor = sensor + } + } + + /** + * Get the contact filtering data. + * + * @return + */ + /** + * Set the contact filtering data. This is an expensive operation and should not be called + * frequently. This will not update contacts until the next time step when either parent body is + * awake. This automatically calls refilter. + * + * @param filter + */ + var filterData: Filter + get() = m_filter + set(filter) { + m_filter.set(filter) + + refilter() + } + + var density: Float + get() = m_density + set(density) { + assert(density >= 0f) + m_density = density + } + + var friction: Float + get() = m_friction + set(value) { + m_friction = value + } + + var restitution: Float + get() = m_restitution + set(value) { + m_restitution = value + } + + private val pool1 = AABB() + private val pool2 = AABB() + private val displacement = Vec2() + + /** + * Call this if you want to establish collision that was previously disabled by + * ContactFilter::ShouldCollide. + */ + fun refilter() { + if (m_body == null) { + return + } + + // Flag associated contacts for filtering. + var edge = m_body!!.getContactList() + while (edge != null) { + val contact = edge.contact + val fixtureA = contact!!.getFixtureA() + val fixtureB = contact.getFixtureB() + if (fixtureA === this || fixtureB === this) { + contact.flagForFiltering() + } + edge = edge.next + } + + val world = m_body!!.world ?: return + +// Touch each proxy so that new pairs may be created + val broadPhase = world.m_contactManager.m_broadPhase + for (i in 0 until m_proxyCount) { + broadPhase.touchProxy(m_proxies!![i].proxyId) + } + } + + /** + * Test a point for containment in this fixture. This only works for convex shapes. + * + * @param p a point in world coordinates. + * @return + */ + fun testPoint(p: Vec2): Boolean { + return m_shape!!.testPoint(m_body!!.xf, p) + } + + /** + * Cast a ray against this shape. + * + * @param output the ray-cast results. + * @param input the ray-cast input parameters. + * @param output + * @param input + */ + fun raycast(output: RayCastOutput, input: RayCastInput, childIndex: Int): Boolean { + return m_shape!!.raycast(output, input, m_body!!.xf, childIndex) + } + + /** + * Get the mass data for this fixture. The mass data is based on the density and the shape. The + * rotational inertia is about the shape's origin. + * + * @return + */ + fun getMassData(massData: MassData) { + m_shape!!.computeMass(massData, m_density) + } + + /** + * Get the fixture's AABB. This AABB may be enlarge and/or stale. If you need a more accurate + * AABB, compute it using the shape and the body transform. + * + * @return + */ + fun getAABB(childIndex: Int): AABB { + assert(childIndex >= 0 && childIndex < m_proxyCount) + return m_proxies!![childIndex].aabb + } + + /** + * Compute the distance from this fixture. + * + * @param p a point in world coordinates. + * @return distance + */ + fun computeDistance(p: Vec2, childIndex: Int, normalOut: Vec2): Float { + return m_shape!!.computeDistanceToOut(m_body!!.transform, p, childIndex, normalOut) + } + + // We need separation create/destroy functions from the constructor/destructor because + // the destructor cannot access the allocator (no destructor arguments allowed by C++). + + fun create(body: Body, def: FixtureDef) { + userData = def.userData + m_friction = def.friction + m_restitution = def.restitution + + this.m_body = body + m_next = null + + + m_filter.set(def.filter) + + m_isSensor = def.isSensor + + m_shape = def.shape!!.clone() + + // Reserve proxy space + val childCount = m_shape!!.getChildCount() + if (m_proxies == null) { + m_proxies = Array(childCount) { FixtureProxy() } + for (i in 0 until childCount) { + m_proxies!![i].fixture = null + m_proxies!![i].proxyId = BroadPhase.NULL_PROXY + } + } + + if (m_proxies!!.size < childCount) { + val old = m_proxies + val newLen = MathUtils.max(old!!.size * 2, childCount) + m_proxies = arrayOfNulls(newLen) as Array + arraycopy(old, 0, m_proxies!!, 0, old.size) + for (i in 0 until newLen) { + if (i >= old.size) { + m_proxies!![i] = FixtureProxy() + } + m_proxies!![i].fixture = null + m_proxies!![i].proxyId = BroadPhase.NULL_PROXY + } + } + m_proxyCount = 0 + + m_density = def.density + } + + fun destroy() { + // The proxies must be destroyed before calling this. + assert(m_proxyCount == 0) + + // Free the child shape. + m_shape = null + m_proxies = null + m_next = null + + // TODO pool shapes + // TODO pool fixtures + } + + // These support body activation/deactivation. + fun createProxies(broadPhase: BroadPhase, xf: Transform) { + assert(m_proxyCount == 0) + + // Create proxies in the broad-phase. + m_proxyCount = m_shape!!.getChildCount() + + for (i in 0 until m_proxyCount) { + val proxy = m_proxies!![i] + m_shape!!.computeAABB(proxy.aabb, xf, i) + proxy.proxyId = broadPhase.createProxy(proxy.aabb, proxy) + proxy.fixture = this + proxy.childIndex = i + } + } + + /** + * Internal method + * + * @param broadPhase + */ + fun destroyProxies(broadPhase: BroadPhase) { + // Destroy proxies in the broad-phase. + for (i in 0 until m_proxyCount) { + val proxy = m_proxies!![i] + broadPhase.destroyProxy(proxy.proxyId) + proxy.proxyId = BroadPhase.NULL_PROXY + } + + m_proxyCount = 0 + } + + /** + * Internal method + * + * @param broadPhase + * @param xf1 + * @param xf2 + */ + fun synchronize(broadPhase: BroadPhase, transform1: Transform, + transform2: Transform) { + if (m_proxyCount == 0) { + return + } + + for (i in 0 until m_proxyCount) { + val proxy = m_proxies!![i] + + // Compute an AABB that covers the swept shape (may miss some rotation effect). + val aabb1 = pool1 + val aab = pool2 + m_shape!!.computeAABB(aabb1, transform1, proxy.childIndex) + m_shape!!.computeAABB(aab, transform2, proxy.childIndex) + + proxy.aabb.lowerBound.x = if (aabb1.lowerBound.x < aab.lowerBound.x) aabb1.lowerBound.x else aab.lowerBound.x + proxy.aabb.lowerBound.y = if (aabb1.lowerBound.y < aab.lowerBound.y) aabb1.lowerBound.y else aab.lowerBound.y + proxy.aabb.upperBound.x = if (aabb1.upperBound.x > aab.upperBound.x) aabb1.upperBound.x else aab.upperBound.x + proxy.aabb.upperBound.y = if (aabb1.upperBound.y > aab.upperBound.y) aabb1.upperBound.y else aab.upperBound.y + displacement.x = transform2.p.x - transform1.p.x + displacement.y = transform2.p.y - transform1.p.y + + broadPhase.moveProxy(proxy.proxyId, proxy.aabb, displacement) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureDef.kt new file mode 100644 index 0000000..312e8bf --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureDef.kt @@ -0,0 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.collision.shapes.Shape +import org.jbox2d.userdata.Box2dTypedUserData + +/** + * A fixture definition is used to create a fixture. This class defines an abstract fixture + * definition. You can reuse fixture definitions safely. + * + * @author daniel + */ +data class FixtureDef( + /** + * The shape, this must be set. The shape will be cloned, so you can create the shape on the + * stack. + */ + var shape: Shape? = null, + + /** + * Use this to store application specific fixture data. + */ + var userData: Any? = null, + + /** + * The friction coefficient, usually in the range [0,1]. + */ + var friction: Float = .2f, + + /** + * The restitution (elasticity) usually in the range [0,1]. + */ + var restitution: Float = 0f, + + /** + * The density, usually in kg/m^2 + */ + var density: Float = 0f, + + /** + * A sensor shape collects contact information but never generates a collision response. + */ + var isSensor: Boolean = false, + + /** + * Contact filtering data; + */ + var filter: Filter = Filter() +) : Box2dTypedUserData by Box2dTypedUserData.Mixin() diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureProxy.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureProxy.kt new file mode 100644 index 0000000..1348551 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/FixtureProxy.kt @@ -0,0 +1,42 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.collision.AABB + +/** + * This proxy is used internally to connect fixtures to the broad-phase. + * + * @author Daniel + */ +class FixtureProxy { + + internal val aabb = AABB() + + internal var fixture: Fixture? = null + + internal var childIndex: Int = 0 + + internal var proxyId: Int = 0 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Island.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Island.kt new file mode 100644 index 0000000..b82a1fa --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Island.kt @@ -0,0 +1,607 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.callbacks.ContactImpulse +import org.jbox2d.callbacks.ContactListener +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Timer +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.contacts.Contact +import org.jbox2d.dynamics.contacts.ContactSolver +import org.jbox2d.dynamics.contacts.ContactSolver.ContactSolverDef +import org.jbox2d.dynamics.contacts.ContactVelocityConstraint +import org.jbox2d.dynamics.contacts.Position +import org.jbox2d.dynamics.contacts.Velocity +import org.jbox2d.dynamics.joints.Joint +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + +/* + Position Correction Notes + ========================= + I tried the several algorithms for position correction of the 2D revolute joint. + I looked at these systems: + - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. + - suspension bridge with 30 1m long planks of length 1m. + - multi-link chain with 30 1m long links. + + Here are the algorithms: + + Baumgarte - A fraction of the position error is added to the velocity error. There is no + separate position solver. + + Pseudo Velocities - After the velocity solver and position integration, + the position error, Jacobian, and effective mass are recomputed. Then + the velocity constraints are solved with pseudo velocities and a fraction + of the position error is added to the pseudo velocity error. The pseudo + velocities are initialized to zero and there is no warm-starting. After + the position solver, the pseudo velocities are added to the positions. + This is also called the First Order World method or the Position LCP method. + + Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the + position error is re-computed for each raint and the positions are updated + after the raint is solved. The radius vectors (aka Jacobians) are + re-computed too (otherwise the algorithm has horrible instability). The pseudo + velocity states are not needed because they are effectively zero at the beginning + of each iteration. Since we have the current position error, we allow the + iterations to terminate early if the error becomes smaller than Settings.linearSlop. + + Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed + each time a raint is solved. + + Here are the results: + Baumgarte - this is the cheapest algorithm but it has some stability problems, + especially with the bridge. The chain links separate easily close to the root + and they jitter as they struggle to pull together. This is one of the most common + methods in the field. The big drawback is that the position correction artificially + affects the momentum, thus leading to instabilities and false bounce. I used a + bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller + factor makes joints and contacts more spongy. + + Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is + stable. However, joints still separate with large angular velocities. Drag the + simple pendulum in a circle quickly and the joint will separate. The chain separates + easily and does not recover. I used a bias factor of 0.2. A larger value lead to + the bridge collapsing when a heavy cube drops on it. + + Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo + Velocities, but in other ways it is worse. The bridge and chain are much more + stable, but the simple pendulum goes unstable at high angular velocities. + + Full NGS - stable in all tests. The joints display good stiffness. The bridge + still sags, but this is better than infinite forces. + + Recommendations + Pseudo Velocities are not really worthwhile because the bridge and chain cannot + recover from joint separation. In other cases the benefit over Baumgarte is small. + + Modified NGS is not a robust method for the revolute joint due to the violent + instability seen in the simple pendulum. Perhaps it is viable with other raint + types, especially scalar constraints where the effective mass is a scalar. + + This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities + and is very fast. I don't think we can escape Baumgarte, especially in highly + demanding cases where high raint fidelity is not needed. + + Full NGS is robust and easy on the eyes. I recommend this as an option for + higher fidelity simulation and certainly for suspension bridges and long chains. + Full NGS might be a good choice for ragdolls, especially motorized ragdolls where + joint separation can be problematic. The number of NGS iterations can be reduced + for better performance without harming robustness much. + + Each joint in a can be handled differently in the position solver. So I recommend + a system where the user can select the algorithm on a per joint basis. I would + probably default to the slower Full NGS and let the user select the faster + Baumgarte method in performance critical scenarios. + */ + +/* + Cache Performance + + The Box2D solvers are dominated by cache misses. Data structures are designed + to increase the number of cache hits. Much of misses are due to random access + to body data. The raint structures are iterated over linearly, which leads + to few cache misses. + + The bodies are not accessed during iteration. Instead read only data, such as + the mass values are stored with the constraints. The mutable data are the raint + impulses and the bodies velocities/positions. The impulses are held inside the + raint structures. The body velocities/positions are held in compact, temporary + arrays to increase the number of cache hits. Linear and angular velocity are + stored in a single array since multiple arrays lead to multiple misses. + */ + +/* + 2D Rotation + + R = [cos(theta) -sin(theta)] + [sin(theta) cos(theta) ] + + thetaDot = omega + + Let q1 = cos(theta), q2 = sin(theta). + R = [q1 -q2] + [q2 q1] + + q1Dot = -thetaDot * q2 + q2Dot = thetaDot * q1 + + q1_new = q1_old - dt * w * q2 + q2_new = q2_old + dt * w * q1 + then normalize. + + This might be faster than computing sin+cos. + However, we can compute sin+cos of the same angle fast. + */ + +/** + * This is an internal class. + * + * @author Daniel Murphy + */ +class Island { + + var m_listener: ContactListener? = null + + + var m_bodies: Array? = null + + var m_contacts: Array? = null + + var m_joints: Array? = null + + + var m_positions: Array? = null + + var m_velocities: Array? = null + + + var m_bodyCount: Int = 0 + + var m_jointCount: Int = 0 + + var m_contactCount: Int = 0 + + + var m_bodyCapacity: Int = 0 + + var m_contactCapacity: Int = 0 + + var m_jointCapacity: Int = 0 + + private val contactSolver = ContactSolver() + private val timer = Timer() + private val solverData = SolverData() + private val solverDef = ContactSolverDef() + + private val toiContactSolver = ContactSolver() + private val toiSolverDef = ContactSolverDef() + + private val impulse = ContactImpulse() + + fun init(bodyCapacity: Int, contactCapacity: Int, jointCapacity: Int, + listener: ContactListener?) { + // System.out.println("Initializing Island"); + m_bodyCapacity = bodyCapacity + m_contactCapacity = contactCapacity + m_jointCapacity = jointCapacity + m_bodyCount = 0 + m_contactCount = 0 + m_jointCount = 0 + + m_listener = listener + + if (m_bodies == null || m_bodyCapacity > m_bodies!!.size) { + m_bodies = arrayOfNulls(m_bodyCapacity) as Array + } + if (m_joints == null || m_jointCapacity > m_joints!!.size) { + m_joints = arrayOfNulls(m_jointCapacity) as Array + } + if (m_contacts == null || m_contactCapacity > m_contacts!!.size) { + m_contacts = arrayOfNulls(m_contactCapacity) as Array + } + + // dynamic array + if (m_velocities == null || m_bodyCapacity > m_velocities!!.size) { + val old = if (m_velocities == null) emptyArray() else m_velocities + m_velocities = arrayOfNulls(m_bodyCapacity) as Array + arraycopy(old!!, 0, m_velocities!!, 0, old!!.size) + for (i in old!!.size until m_velocities!!.size) { + m_velocities!![i] = Velocity() + } + } + + // dynamic array + if (m_positions == null || m_bodyCapacity > m_positions!!.size) { + val old = if (m_positions == null) emptyArray() else m_positions + m_positions = arrayOfNulls(m_bodyCapacity) as Array + arraycopy(old!!, 0, m_positions!!, 0, old!!.size) + for (i in old!!.size until m_positions!!.size) { + m_positions!![i] = Position() + } + } + } + + fun clear() { + m_bodyCount = 0 + m_contactCount = 0 + m_jointCount = 0 + } + + fun solve(profile: Profile, step: TimeStep, gravity: Vec2, allowSleep: Boolean) { + + // System.out.println("Solving Island"); + val h = step.dt + + // Integrate velocities and apply damping. Initialize the body state. + for (i in 0 until m_bodyCount) { + val b = m_bodies!![i] + val bm_sweep = b.sweep + val c = bm_sweep.c + val a = bm_sweep.a + val v = b._linearVelocity + var w = b._angularVelocity + + // Store positions for continuous collision. + bm_sweep.c0.set(bm_sweep.c) + bm_sweep.a0 = bm_sweep.a + + if (b._type === BodyType.DYNAMIC) { + // Integrate velocities. + // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force); + v.x += h * (b.gravityScale * gravity.x + b.m_invMass * b.force.x) + v.y += h * (b.gravityScale * gravity.y + b.m_invMass * b.force.y) + w += h * b.m_invI * b.torque + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * + // exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + v.x *= 1.0f / (1.0f + h * b.m_linearDamping) + v.y *= 1.0f / (1.0f + h * b.m_linearDamping) + w *= 1.0f / (1.0f + h * b.m_angularDamping) + } + + m_positions!![i].c.x = c.x + m_positions!![i].c.y = c.y + m_positions!![i].a = a + m_velocities!![i].v.x = v.x + m_velocities!![i].v.y = v.y + m_velocities!![i].w = w + } + + timer.reset() + + // Solver data + solverData.step = step + solverData.positions = m_positions + solverData.velocities = m_velocities + + // Initialize velocity constraints. + solverDef.step = step + solverDef.contacts = m_contacts + solverDef.count = m_contactCount + solverDef.positions = m_positions + solverDef.velocities = m_velocities + + contactSolver.init(solverDef) + // System.out.println("island init vel"); + contactSolver.initializeVelocityConstraints() + + if (step.warmStarting) { + // System.out.println("island warm start"); + contactSolver.warmStart() + } + + for (i in 0 until m_jointCount) { + m_joints!![i].initVelocityConstraints(solverData) + } + + profile.solveInit.accum(timer.milliseconds) + + // Solve velocity constraints + timer.reset() + // System.out.println("island solving velocities"); + for (i in 0 until step.velocityIterations) { + for (j in 0 until m_jointCount) { + m_joints!![j].solveVelocityConstraints(solverData) + } + + contactSolver.solveVelocityConstraints() + } + + // Store impulses for warm starting + contactSolver.storeImpulses() + profile.solveVelocity.accum(timer.milliseconds) + + // Integrate positions + for (i in 0 until m_bodyCount) { + val c = m_positions!![i].c + var a = m_positions!![i].a + val v = m_velocities!![i].v + var w = m_velocities!![i].w + + // Check for large velocities + val translationx = v.x * h + val translationy = v.y * h + + if (translationx * translationx + translationy * translationy > Settings.maxTranslationSquared) { + val ratio = Settings.maxTranslation / MathUtils.sqrt(translationx * translationx + translationy * translationy) + v.x *= ratio + v.y *= ratio + } + + val rotation = h * w + if (rotation * rotation > Settings.maxRotationSquared) { + val ratio = Settings.maxRotation / MathUtils.abs(rotation) + w *= ratio + } + + // Integrate + c.x += h * v.x + c.y += h * v.y + a += h * w + + m_positions!![i].a = a + m_velocities!![i].w = w + } + + // Solve position constraints + timer.reset() + var positionSolved = false + for (i in 0 until step.positionIterations) { + val contactsOkay = contactSolver.solvePositionConstraints() + + var jointsOkay = true + for (j in 0 until m_jointCount) { + val jointOkay = m_joints!![j].solvePositionConstraints(solverData) + jointsOkay = jointsOkay && jointOkay + } + + if (contactsOkay && jointsOkay) { + // Exit early if the position errors are small. + positionSolved = true + break + } + } + + // Copy state buffers back to the bodies + for (i in 0 until m_bodyCount) { + val body = m_bodies!![i] + body.sweep.c.x = m_positions!![i].c.x + body.sweep.c.y = m_positions!![i].c.y + body.sweep.a = m_positions!![i].a + body._linearVelocity.x = m_velocities!![i].v.x + body._linearVelocity.y = m_velocities!![i].v.y + body._angularVelocity = m_velocities!![i].w + body.synchronizeTransform() + } + + profile.solvePosition.accum(timer.milliseconds) + + report(contactSolver.m_velocityConstraints) + + if (allowSleep) { + var minSleepTime = Float.MAX_VALUE + + val linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance + val angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance + + for (i in 0 until m_bodyCount) { + val b = m_bodies!![i] + if (b.type === BodyType.STATIC) { + continue + } + + if (b.flags and Body.e_autoSleepFlag == 0 + || b._angularVelocity * b._angularVelocity > angTolSqr + || Vec2.dot(b._linearVelocity, b._linearVelocity) > linTolSqr) { + b.m_sleepTime = 0.0f + minSleepTime = 0.0f + } else { + b.m_sleepTime += h + minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime) + } + } + + if (minSleepTime >= Settings.timeToSleep && positionSolved) { + for (i in 0 until m_bodyCount) { + val b = m_bodies!![i] + b.isAwake = false + } + } + } + } + + fun solveTOI(subStep: TimeStep, toiIndexA: Int, toiIndexB: Int) { + assert(toiIndexA < m_bodyCount) + assert(toiIndexB < m_bodyCount) + + // Initialize the body state. + for (i in 0 until m_bodyCount) { + m_positions!![i].c.x = m_bodies!![i].sweep.c.x + m_positions!![i].c.y = m_bodies!![i].sweep.c.y + m_positions!![i].a = m_bodies!![i].sweep.a + m_velocities!![i].v.x = m_bodies!![i]._linearVelocity.x + m_velocities!![i].v.y = m_bodies!![i]._linearVelocity.y + m_velocities!![i].w = m_bodies!![i]._angularVelocity + } + + toiSolverDef.contacts = m_contacts + toiSolverDef.count = m_contactCount + toiSolverDef.step = subStep + toiSolverDef.positions = m_positions + toiSolverDef.velocities = m_velocities + toiContactSolver.init(toiSolverDef) + + // Solve position constraints. + for (i in 0 until subStep.positionIterations) { + val contactsOkay = toiContactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB) + if (contactsOkay) { + break + } + } + // #if 0 + // // Is the new position really safe? + // for (int i = 0; i < m_contactCount; ++i) + // { + // Contact* c = m_contacts[i]; + // Fixture* fA = c.GetFixtureA(); + // Fixture* fB = c.GetFixtureB(); + // + // Body bA = fA.GetBody(); + // Body bB = fB.GetBody(); + // + // int indexA = c.GetChildIndexA(); + // int indexB = c.GetChildIndexB(); + // + // DistanceInput input; + // input.proxyA.Set(fA.GetShape(), indexA); + // input.proxyB.Set(fB.GetShape(), indexB); + // input.transformA = bA.GetTransform(); + // input.transformB = bB.GetTransform(); + // input.useRadii = false; + // + // DistanceOutput output; + // SimplexCache cache; + // cache.count = 0; + // Distance(&output, &cache, &input); + // + // if (output.distance == 0 || cache.count == 3) + // { + // cache.count += 0; + // } + // } + // #endif + + // Leap of faith to new safe state. + m_bodies!![toiIndexA].sweep.c0.x = m_positions!![toiIndexA].c.x + m_bodies!![toiIndexA].sweep.c0.y = m_positions!![toiIndexA].c.y + m_bodies!![toiIndexA].sweep.a0 = m_positions!![toiIndexA].a + m_bodies!![toiIndexB].sweep.c0.set(m_positions!![toiIndexB].c) + m_bodies!![toiIndexB].sweep.a0 = m_positions!![toiIndexB].a + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + toiContactSolver.initializeVelocityConstraints() + + // Solve velocity constraints. + for (i in 0 until subStep.velocityIterations) { + toiContactSolver.solveVelocityConstraints() + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + val h = subStep.dt + + // Integrate positions + for (i in 0 until m_bodyCount) { + val c = m_positions!![i].c + var a = m_positions!![i].a + val v = m_velocities!![i].v + var w = m_velocities!![i].w + + // Check for large velocities + val translationx = v.x * h + val translationy = v.y * h + if (translationx * translationx + translationy * translationy > Settings.maxTranslationSquared) { + val ratio = Settings.maxTranslation / MathUtils.sqrt(translationx * translationx + translationy * translationy) + v.mulLocal(ratio) + } + + val rotation = h * w + if (rotation * rotation > Settings.maxRotationSquared) { + val ratio = Settings.maxRotation / MathUtils.abs(rotation) + w *= ratio + } + + // Integrate + c.x += v.x * h + c.y += v.y * h + a += h * w + + m_positions!![i].c.x = c.x + m_positions!![i].c.y = c.y + m_positions!![i].a = a + m_velocities!![i].v.x = v.x + m_velocities!![i].v.y = v.y + m_velocities!![i].w = w + + // Sync bodies + val body = m_bodies!![i] + body.sweep.c.x = c.x + body.sweep.c.y = c.y + body.sweep.a = a + body._linearVelocity.x = v.x + body._linearVelocity.y = v.y + body._angularVelocity = w + body.synchronizeTransform() + } + + report(toiContactSolver.m_velocityConstraints) + } + + fun add(body: Body) { + assert(m_bodyCount < m_bodyCapacity) + body.islandIndex = m_bodyCount + m_bodies!![m_bodyCount] = body + ++m_bodyCount + } + + fun add(contact: Contact) { + assert(m_contactCount < m_contactCapacity) + m_contacts!![m_contactCount++] = contact + } + + fun add(joint: Joint) { + assert(m_jointCount < m_jointCapacity) + m_joints!![m_jointCount++] = joint + } + + fun report(constraints: Array) { + if (m_listener == null) { + return + } + + for (i in 0 until m_contactCount) { + val c = m_contacts!![i] + + val vc = constraints[i] + impulse.count = vc.pointCount + for (j in 0 until vc.pointCount) { + impulse.normalImpulses[j] = vc.points[j].normalImpulse + impulse.tangentImpulses[j] = vc.points[j].tangentImpulse + } + + m_listener!!.postSolve(c, impulse) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Profile.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Profile.kt new file mode 100644 index 0000000..9c0946d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/Profile.kt @@ -0,0 +1,107 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.common.MathUtils + +class Profile { + + + val step = ProfileEntry() + + val stepInit = ProfileEntry() + + val collide = ProfileEntry() + + val solveParticleSystem = ProfileEntry() + + val solve = ProfileEntry() + + val solveInit = ProfileEntry() + + val solveVelocity = ProfileEntry() + + val solvePosition = ProfileEntry() + + val broadphase = ProfileEntry() + + val solveTOI = ProfileEntry() + + class ProfileEntry { + internal var longAvg: Float = 0.toFloat() + internal var shortAvg: Float = 0.toFloat() + internal var min: Float = 0.toFloat() + internal var max: Float = 0.toFloat() + internal var accum: Float = 0.toFloat() + + init { + min = Float.MAX_VALUE + max = -Float.MAX_VALUE + } + + fun record(value: Float) { + longAvg = longAvg * (1 - LONG_FRACTION) + value * LONG_FRACTION + shortAvg = shortAvg * (1 - SHORT_FRACTION) + value * SHORT_FRACTION + min = MathUtils.min(value, min) + max = MathUtils.max(value, max) + } + + fun startAccum() { + accum = 0f + } + + fun accum(value: Float) { + accum += value + } + + fun endAccum() { + record(accum) + } + + override fun toString(): String { + return "$shortAvg ($longAvg) [$min,$max]" + } + } + + fun toDebugStrings(strings: MutableList) { + strings.add("Profile:") + strings.add(" step: $step") + strings.add(" init: $stepInit") + strings.add(" collide: $collide") + strings.add(" particles: $solveParticleSystem") + strings.add(" solve: $solve") + strings.add(" solveInit: $solveInit") + strings.add(" solveVelocity: $solveVelocity") + strings.add(" solvePosition: $solvePosition") + strings.add(" broadphase: $broadphase") + strings.add(" solveTOI: $solveTOI") + } + + companion object { + private val LONG_AVG_NUMS = 20 + private val LONG_FRACTION = 1f / LONG_AVG_NUMS + private val SHORT_AVG_NUMS = 5 + private val SHORT_FRACTION = 1f / SHORT_AVG_NUMS + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/SolverData.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/SolverData.kt new file mode 100644 index 0000000..c743b29 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/SolverData.kt @@ -0,0 +1,36 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.dynamics.contacts.Position +import org.jbox2d.dynamics.contacts.Velocity + +class SolverData { + + var step: TimeStep? = null + + var positions: Array? = null + + var velocities: Array? = null +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/TimeStep.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/TimeStep.kt new file mode 100644 index 0000000..0eb0390 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/TimeStep.kt @@ -0,0 +1,52 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +//updated to rev 100 +/** + * This is an internal structure. + */ +class TimeStep { + + /** time step */ + + var dt: Float = 0.toFloat() + + /** inverse time step (0 if dt == 0). */ + + var inv_dt: Float = 0.toFloat() + + /** dt * inv_dt0 */ + + var dtRatio: Float = 0.toFloat() + + + var velocityIterations: Int = 0 + + + var positionIterations: Int = 0 + + + var warmStarting: Boolean = false +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/World.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/World.kt new file mode 100644 index 0000000..b1f858b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/World.kt @@ -0,0 +1,1926 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics + +import org.jbox2d.callbacks.* +import org.jbox2d.collision.* +import org.jbox2d.collision.broadphase.* +import org.jbox2d.collision.shapes.* +import org.jbox2d.common.* +import org.jbox2d.dynamics.contacts.* +import org.jbox2d.dynamics.joints.* +import org.jbox2d.internal.* +import org.jbox2d.particle.* +import org.jbox2d.pooling.* +import org.jbox2d.pooling.arrays.* +import org.jbox2d.pooling.normal.* +import org.jbox2d.userdata.* + +/** + * The world class manages all physics entities, dynamic simulation, and asynchronous queries. The + * world also contains efficient memory management facilities. + * + * @author Daniel Murphy + */ +open class World(gravity: Vec2, val pool: IWorldPool, broadPhase: BroadPhase) : WorldRef, Box2dTypedUserData by Box2dTypedUserData.Mixin() { + var customScale: Double = 1.0 + + override val world: World get() = this + + // statistics gathering + var activeContacts = 0 + var contactPoolCount = 0 + + var userData: Any? = null + + var m_flags: Int = CLEAR_FORCES + + /** + * Get the contact manager for testing purposes + * + * @return + */ + var m_contactManager: ContactManager = ContactManager(this, broadPhase) + protected set + + /** + * Get the world body list. With the returned body, use Body.getNext to get the next body in the + * world list. A null body indicates the end of the list. + * + * @return the head of the world body list. + */ + var bodyList: Body? = null + private set + /** + * Get the world joint list. With the returned joint, use Joint.getNext to get the next joint in + * the world list. A null joint indicates the end of the list. + * + * @return the head of the world joint list. + */ + var jointList: Joint? = null + private set + + /** + * Get the number of bodies. + * + * @return + */ + var bodyCount: Int = 0 + private set + /** + * Get the number of joints. + * + * @return + */ + var jointCount: Int = 0 + private set + + /** + * Get the global gravity vector. + * + * @return + */ + /** + * Change the global gravity vector. + * + * @param gravity + */ + var gravity = Vec2() + set(gravity) { + this.gravity.set(gravity) + } + var isSleepingAllowed: Boolean = true + + // private Body m_groundBody; + + /** + * Register a destruction listener. The listener is owned by you and must remain in scope. + * + * @param listener + */ + var destructionListener: DestructionListener? = null + var particleDestructionListener: ParticleDestructionListener? = null + private var m_debugDraw: DebugDraw? = null + + /** + * This is used to compute the time step ratio to support a variable time step. + */ + private var m_inv_dt0: Float = 0f + + // these are for debugging the solver + /** + * Enable/disable warm starting. For testing. + * + * @param flag + */ + var isWarmStarting: Boolean = true + /** + * Enable/disable continuous physics. For testing. + * + * @param flag + */ + var isContinuousPhysics: Boolean = true + var isSubStepping: Boolean = false + + private var m_stepComplete: Boolean = true + + val profile: Profile = Profile() + + private val m_particleSystem: ParticleSystem = ParticleSystem(this) + + + private val contactStacks = Array>(ShapeType.values().size) { arrayOfNulls(ShapeType.values().size) as Array } + + var isAllowSleep: Boolean + get() = isSleepingAllowed + set(flag) { + if (flag == isSleepingAllowed) { + return + } + + isSleepingAllowed = flag + if (!isSleepingAllowed) { + var b = bodyList + while (b != null) { + b.isAwake = true + b = b.m_next + } + } + } + + // djm pooling + private val step = TimeStep() + private val stepTimer = Timer() + private val tempTimer = Timer() + + private val color = Color3f() + private val xf = Transform() + private val cA = Vec2() + private val cB = Vec2() + private val avs = Vec2ArrayPool() + + private val wqwrapper = WorldQueryWrapper() + + private val wrcwrapper = WorldRayCastWrapper() + private val input = RayCastInput() + + /** + * Get the world contact list. With the returned contact, use Contact.getNext to get the next + * contact in the world list. A null contact indicates the end of the list. + * + * @return the head of the world contact list. + * @warning contacts are created and destroyed in the middle of a time step. Use ContactListener + * to avoid missing contacts. + */ + val contactList: Contact + get() = m_contactManager!!.m_contactList!! + + + /** + * Get the number of broad-phase proxies. + * + * @return + */ + val proxyCount: Int + get() = m_contactManager.m_broadPhase.proxyCount + + /** + * Get the number of contacts (each may have 0 or more contact points). + * + * @return + */ + val contactCount: Int + get() = m_contactManager.m_contactCount + + /** + * Gets the height of the dynamic tree + * + * @return + */ + val treeHeight: Int + get() = m_contactManager.m_broadPhase.treeHeight + + /** + * Gets the balance of the dynamic tree + * + * @return + */ + val treeBalance: Int + get() = m_contactManager.m_broadPhase.treeBalance + + /** + * Gets the quality of the dynamic tree + * + * @return + */ + val treeQuality: Float + get() = m_contactManager.m_broadPhase.treeQuality + + /** + * Is the world locked (in the middle of a time step). + * + * @return + */ + val isLocked: Boolean + get() = m_flags and LOCKED == LOCKED + + /** + * Get the flag that controls automatic clearing of forces after each time step. + * + * @return + */ + /** + * Set flag to control automatic clearing of forces after each time step. + * + * @param flag + */ + var autoClearForces: Boolean + get() = m_flags and CLEAR_FORCES == CLEAR_FORCES + set(flag) = if (flag) { + m_flags = m_flags or CLEAR_FORCES + } else { + m_flags = m_flags and CLEAR_FORCES.inv() + } + + private val island = Island() + private var stack = arrayOfNulls(10) // TODO djm find a good initial stack number; + private val broadphaseTimer = Timer() + + private val toiIsland = Island() + private val toiInput = TimeOfImpact.TOIInput() + private val toiOutput = TimeOfImpact.TOIOutput() + private val subStep = TimeStep() + private val tempBodies = arrayOfNulls(2) + private val backup1 = Sweep() + private val backup2 = Sweep() + private val liquidLength = .12f + private var averageLinearVel = -1f + private val liquidOffset = Vec2() + private val circCenterMoved = Vec2() + private val liquidColor = Color3f(.4f, .4f, 1f) + + private val center = Vec2() + private val axis = Vec2() + private val v1 = Vec2() + private val v2 = Vec2() + private val tlvertices = Vec2ArrayPool() + + /** + * Get the world particle group list. With the returned group, use ParticleGroup::GetNext to get + * the next group in the world list. A NULL group indicates the end of the list. + * + * @return the head of the world particle group list. + */ + val particleGroupList: Array + get() = m_particleSystem.getParticleGroupList()!! + + /** + * Get the number of particle groups. + * + * @return + */ + val particleGroupCount: Int + get() = m_particleSystem.particleGroupCount + + /** + * Get the number of particles. + * + * @return + */ + val particleCount: Int + get() = m_particleSystem.particleCount + + /** + * Get the maximum number of particles. + * + * @return + */ + /** + * Set the maximum number of particles. + * + * @param count + */ + var particleMaxCount: Int + get() = m_particleSystem.particleMaxCount + set(count) { + m_particleSystem.particleMaxCount = count + } + + /** + * Get the particle density. + * + * @return + */ + /** + * Change the particle density. + * + * @param density + */ + var particleDensity: Float + get() = m_particleSystem.particleDensity + set(density) { + m_particleSystem.particleDensity = density + } + + /** + * Get the particle gravity scale. + * + * @return + */ + /** + * Change the particle gravity scale. Adjusts the effect of the global gravity vector on + * particles. Default value is 1.0f. + * + * @param gravityScale + */ + var particleGravityScale: Float + get() = m_particleSystem.particleGravityScale + set(gravityScale) { + m_particleSystem.particleGravityScale = gravityScale + + } + + /** + * Get damping for particles + * + * @return + */ + /** + * Damping is used to reduce the velocity of particles. The damping parameter can be larger than + * 1.0f but the damping effect becomes sensitive to the time step when the damping parameter is + * large. + * + * @param damping + */ + var particleDamping: Float + get() = m_particleSystem.particleDamping + set(damping) { + m_particleSystem.particleDamping = damping + } + + /** + * Get the particle radius. + * + * @return + */ + /** + * Change the particle radius. You should set this only once, on world start. If you change the + * radius during execution, existing particles may explode, shrink, or behave unexpectedly. + * + * @param radius + */ + var particleRadius: Float + get() = m_particleSystem.particleRadius + set(radius) { + m_particleSystem.particleRadius = radius + } + + /** + * Get the particle data. @return the pointer to the head of the particle data. + * + * @return + */ + val particleFlagsBuffer: IntArray + get() = m_particleSystem!!.particleFlagsBuffer!! + + val particlePositionBuffer: Array + get() = m_particleSystem!!.particlePositionBuffer!! + + val particleVelocityBuffer: Array + get() = m_particleSystem.particleVelocityBuffer!! + + val particleColorBuffer: Array + get() = m_particleSystem.particleColorBuffer!! + + val particleGroupBuffer: Array + get() = m_particleSystem.particleGroupBuffer!! + + val particleUserDataBuffer: Array + get() = m_particleSystem.particleUserDataBuffer!! + + /** + * Get contacts between particles + * + * @return + */ + val particleContacts: Array + get() = m_particleSystem.m_contactBuffer + + val particleContactCount: Int + get() = m_particleSystem.m_contactCount + + /** + * Get contacts between particles and bodies + * + * @return + */ + val particleBodyContacts: Array + get() = m_particleSystem.m_bodyContactBuffer + + val particleBodyContactCount: Int + get() = m_particleSystem.m_bodyContactCount + + + constructor( + gravity: Vec2, + pool: IWorldPool = DefaultWorldPool(WORLD_POOL_SIZE, WORLD_POOL_CONTAINER_SIZE), + strategy: BroadPhaseStrategy = DynamicTree() + ) : this(gravity, pool, DefaultBroadPhaseBuffer(strategy)) + + constructor( + gravityX: Number = 0f, + gravityY: Number = 9.8f, + pool: IWorldPool = DefaultWorldPool(WORLD_POOL_SIZE, WORLD_POOL_CONTAINER_SIZE), + strategy: BroadPhaseStrategy = DynamicTree() + ) : this(Vec2(gravityX.toFloat(), gravityY.toFloat()), pool, DefaultBroadPhaseBuffer(strategy)) + + init { + this.gravity.set(gravity) + initializeRegisters() + } + + private fun addType(creator: IDynamicStack, type1: ShapeType, type2: ShapeType) { + val register = ContactRegister() + register.creator = creator + register.primary = true + contactStacks[type1.ordinal][type2.ordinal] = register + + if (type1 !== type2) { + val register2 = ContactRegister() + register2.creator = creator + register2.primary = false + contactStacks[type2.ordinal][type1.ordinal] = register2 + } + } + + private fun initializeRegisters() { + addType(pool.circleContactStack, ShapeType.CIRCLE, ShapeType.CIRCLE) + addType(pool.polyCircleContactStack, ShapeType.POLYGON, ShapeType.CIRCLE) + addType(pool.polyContactStack, ShapeType.POLYGON, ShapeType.POLYGON) + addType(pool.edgeCircleContactStack, ShapeType.EDGE, ShapeType.CIRCLE) + addType(pool.edgePolyContactStack, ShapeType.EDGE, ShapeType.POLYGON) + addType(pool.chainCircleContactStack, ShapeType.CHAIN, ShapeType.CIRCLE) + addType(pool.chainPolyContactStack, ShapeType.CHAIN, ShapeType.POLYGON) + } + + fun popContact(fixtureA: Fixture, indexA: Int, fixtureB: Fixture, indexB: Int): Contact? { + val type1 = fixtureA.type + val type2 = fixtureB.type + + val reg = contactStacks[type1.ordinal][type2.ordinal] + if (reg != null) { + if (reg.primary) { + val c = reg.creator!!.pop() + c.init(fixtureA, indexA, fixtureB, indexB) + return c + } else { + val c = reg.creator!!.pop() + c.init(fixtureB, indexB, fixtureA, indexA) + return c + } + } else { + return null + } + } + + fun pushContact(contact: Contact) { + val fixtureA = contact.getFixtureA() + val fixtureB = contact.getFixtureB() + + if (contact.m_manifold.pointCount > 0 && !fixtureA!!.isSensor && !fixtureB!!.isSensor) { + fixtureA.getBody()!!.isAwake = true + fixtureB.getBody()!!.isAwake = true + } + + val type1 = fixtureA!!.type + val type2 = fixtureB!!.type + + val creator = contactStacks[type1.ordinal][type2.ordinal].creator + creator!!.push(contact) + } + + /** + * Register a contact filter to provide specific control over collision. Otherwise the default + * filter is used (_defaultFilter). The listener is owned by you and must remain in scope. + * + * @param filter + */ + fun setContactFilter(filter: ContactFilter) { + m_contactManager.m_contactFilter = filter + } + + /** + * Register a contact event listener. The listener is owned by you and must remain in scope. + * + * @param listener + */ + fun setContactListener(listener: ContactListener) { + m_contactManager.m_contactListener = listener + } + + /** + * Register a routine for debug drawing. The debug draw functions are called inside with + * World.DrawDebugData method. The debug draw object is owned by you and must remain in scope. + * + * @param debugDraw + */ + fun setDebugDraw(debugDraw: DebugDraw) { + m_debugDraw = debugDraw + } + + /** + * create a rigid body given a definition. No reference to the definition is retained. + * + * @warning This function is locked during callbacks. + * @param def + * @return + */ + fun createBody(def: BodyDef): Body { + assert(!isLocked) + if (isLocked) { + error("World is locked") + } + // TODO djm pooling + val b = Body(def, this) + + // add to world doubly linked list + b.prev = null + b.m_next = bodyList + if (bodyList != null) { + bodyList!!.prev = b + } + bodyList = b + ++bodyCount + + return b + } + + /** + * destroy a rigid body given a definition. No reference to the definition is retained. This + * function is locked during callbacks. + * + * @warning This automatically deletes all associated shapes and joints. + * @warning This function is locked during callbacks. + * @param body + */ + fun destroyBody(body: Body) { + assert(bodyCount > 0) + assert(!isLocked) + if (isLocked) { + return + } + + // Delete the attached joints. + var je = body.m_jointList + while (je != null) { + val je0 = je + je = je.next + if (destructionListener != null) { + destructionListener!!.sayGoodbye(je0.joint!!) + } + + destroyJoint(je0.joint) + + body.m_jointList = je + } + body.m_jointList = null + + // Delete the attached contacts. + var ce = body.m_contactList + while (ce != null) { + val ce0 = ce + ce = ce.next + m_contactManager.destroy(ce0.contact!!) + } + body.m_contactList = null + + var f = body.m_fixtureList + while (f != null) { + val f0 = f + f = f.m_next + + if (destructionListener != null) { + destructionListener!!.sayGoodbye(f0) + } + + f0.destroyProxies(m_contactManager.m_broadPhase) + f0.destroy() + // TODO djm recycle fixtures (here or in that destroy method) + body.m_fixtureList = f + body.m_fixtureCount -= 1 + } + body.m_fixtureList = null + body.m_fixtureCount = 0 + + // Remove world body list. + if (body.prev != null) { + body.prev!!.m_next = body.m_next + } + + if (body.m_next != null) { + body.m_next!!.prev = body.prev + } + + if (body == bodyList) { + bodyList = body.m_next + } + + --bodyCount + // TODO djm recycle body + } + + /** + * create a joint to constrain bodies together. No reference to the definition is retained. This + * may cause the connected bodies to cease colliding. + * + * @warning This function is locked during callbacks. + * @param def + * @return + */ + fun createJoint(def: JointDef): Joint? { + assert(!isLocked) + if (isLocked) { + return null + } + + val j = Joint.create(this, def) + + // Connect to the world list. + j!!.prev = null + j.next = jointList + if (jointList != null) { + jointList!!.prev = j + } + jointList = j + ++jointCount + + // Connect to the bodies' doubly linked lists. + j.edgeA.joint = j + j.edgeA.other = j.bodyB + j.edgeA.prev = null + j.edgeA.next = j.bodyA!!.m_jointList + if (j.bodyA!!.m_jointList != null) { + j.bodyA!!.m_jointList!!.prev = j.edgeA + } + j.bodyA!!.m_jointList = j.edgeA + + j.edgeB.joint = j + j.edgeB.other = j.bodyA + j.edgeB.prev = null + j.edgeB.next = j.bodyB!!.m_jointList + if (j.bodyB!!.m_jointList != null) { + j.bodyB!!.m_jointList!!.prev = j.edgeB + } + j.bodyB!!.m_jointList = j.edgeB + + val bodyA = def.bodyA + val bodyB = def.bodyB + + // If the joint prevents collisions, then flag any contacts for filtering. + if (!def.collideConnected) { + var edge = bodyB!!.getContactList() + while (edge != null) { + if (edge.other == bodyA) { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.contact!!.flagForFiltering() + } + + edge = edge.next + } + } + + // Note: creating a joint doesn't wake the bodies. + + return j + } + + /** + * destroy a joint. This may cause the connected bodies to begin colliding. + * + * @warning This function is locked during callbacks. + * @param joint + */ + fun destroyJoint(j: Joint?) { + assert(!isLocked) + if (isLocked) { + return + } + + val collideConnected = j!!.getCollideConnected() + + // Remove from the doubly linked list. + if (j.prev != null) { + j.prev!!.next = j.next + } + + if (j.next != null) { + j.next!!.prev = j.prev + } + + if (j === jointList) { + jointList = j.next + } + + // Disconnect from island graph. + val bodyA = j.bodyA + val bodyB = j.bodyB + + // Wake up connected bodies. + bodyA!!.isAwake = true + bodyB!!.isAwake = true + + // Remove from body 1. + if (j.edgeA.prev != null) { + j.edgeA.prev!!.next = j.edgeA.next + } + + if (j.edgeA.next != null) { + j.edgeA.next!!.prev = j.edgeA.prev + } + + if (j.edgeA == bodyA.m_jointList) { + bodyA.m_jointList = j.edgeA.next + } + + j.edgeA.prev = null + j.edgeA.next = null + + // Remove from body 2 + if (j.edgeB.prev != null) { + j.edgeB.prev!!.next = j.edgeB.next + } + + if (j.edgeB.next != null) { + j.edgeB.next!!.prev = j.edgeB.prev + } + + if (j.edgeB == bodyB.m_jointList) { + bodyB.m_jointList = j.edgeB.next + } + + j.edgeB.prev = null + j.edgeB.next = null + + Joint.destroy(j) + + assert(jointCount > 0) + --jointCount + + // If the joint prevents collisions, then flag any contacts for filtering. + if (!collideConnected) { + var edge = bodyB.getContactList() + while (edge != null) { + if (edge.other == bodyA) { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge.contact!!.flagForFiltering() + } + + edge = edge.next + } + } + } + + /** + * Take a time step. This performs collision detection, integration, and constraint solution. + * + * @param timeStep the amount of time to simulate, this should not vary. + * @param velocityIterations for the velocity constraint solver. + * @param positionIterations for the position constraint solver. + */ + fun step(dt: Float, velocityIterations: Int, positionIterations: Int) { + stepTimer.reset() + tempTimer.reset() + // log.debug("Starting step"); + // If new fixtures were added, we need to find the new contacts. + if (m_flags and NEW_FIXTURE == NEW_FIXTURE) { + // log.debug("There's a new fixture, lets look for new contacts"); + m_contactManager.findNewContacts() + m_flags = m_flags and NEW_FIXTURE.inv() + } + + m_flags = m_flags or LOCKED + + step.dt = dt + step.velocityIterations = velocityIterations + step.positionIterations = positionIterations + if (dt > 0.0f) { + step.inv_dt = 1.0f / dt + } else { + step.inv_dt = 0.0f + } + + step.dtRatio = m_inv_dt0 * dt + + step.warmStarting = isWarmStarting + profile.stepInit.record(tempTimer.milliseconds) + + // Update contacts. This is where some contacts are destroyed. + tempTimer.reset() + m_contactManager.collide() + profile.collide.record(tempTimer.milliseconds) + + // Integrate velocities, solve velocity constraints, and integrate positions. + if (m_stepComplete && step.dt > 0.0f) { + tempTimer.reset() + m_particleSystem.solve(step) // Particle Simulation + profile.solveParticleSystem.record(tempTimer.milliseconds) + tempTimer.reset() + solve(step) + profile.solve.record(tempTimer.milliseconds) + } + + // Handle TOI events. + if (isContinuousPhysics && step.dt > 0.0f) { + tempTimer.reset() + solveTOI(step) + profile.solveTOI.record(tempTimer.milliseconds) + } + + if (step.dt > 0.0f) { + m_inv_dt0 = step.inv_dt + } + + if (m_flags and CLEAR_FORCES == CLEAR_FORCES) { + clearForces() + } + + m_flags = m_flags and LOCKED.inv() + // log.debug("ending step"); + + profile.step.record(stepTimer.milliseconds) + } + + /** + * Call this after you are done with time steps to clear the forces. You normally call this after + * each call to Step, unless you are performing sub-steps. By default, forces will be + * automatically cleared, so you don't need to call this function. + * + * @see setAutoClearForces + */ + fun clearForces() { + var body = bodyList + while (body != null) { + body.force.setZero() + body.torque = 0.0f + body = body.getNext() + } + } + + /** + * Call this to draw shapes and other debug draw data. + */ + fun drawDebugData() { + if (m_debugDraw == null) { + return + } + + + val flags = m_debugDraw!!.flags + val wireframe = flags and DebugDraw.e_wireframeDrawingBit != 0 + + if (flags and DebugDraw.e_shapeBit != 0) { + var b = bodyList + while (b != null) { + xf.set(b.transform) + var f = b.getFixtureList() + while (f != null) { + if (!b.isActive) { + color.set(0.5f, 0.5f, 0.3f) + drawShape(f, xf, color, wireframe) + } else if (b.type === BodyType.STATIC) { + color.set(0.5f, 0.9f, 0.3f) + drawShape(f, xf, color, wireframe) + } else if (b.type === BodyType.KINEMATIC) { + color.set(0.5f, 0.5f, 0.9f) + drawShape(f, xf, color, wireframe) + } else if (!b.isAwake) { + color.set(0.5f, 0.5f, 0.5f) + drawShape(f, xf, color, wireframe) + } else { + color.set(0.9f, 0.7f, 0.7f) + drawShape(f, xf, color, wireframe) + } + f = f.getNext() + } + b = b.getNext() + } + drawParticleSystem(m_particleSystem) + } + + if (flags and DebugDraw.e_jointBit != 0) { + var j = jointList + while (j != null) { + drawJoint(j) + j = j.next + } + } + + if (flags and DebugDraw.e_pairBit != 0) { + color.set(0.3f, 0.9f, 0.9f) + var c: Contact? = m_contactManager.m_contactList + while (c != null) { + val fixtureA = c.getFixtureA() + val fixtureB = c.getFixtureB() + fixtureA!!.getAABB(c.getChildIndexA()).getCenterToOut(cA) + fixtureB!!.getAABB(c.getChildIndexB()).getCenterToOut(cB) + m_debugDraw!!.drawSegment(cA, cB, color) + c = c.getNext() + } + } + + if (flags and DebugDraw.e_aabbBit != 0) { + color.set(0.9f, 0.3f, 0.9f) + + var b = bodyList + while (b != null) { + if (!b.isActive) { + b = b.getNext() + continue + } + + var f = b.getFixtureList() + while (f != null) { + for (i in 0 until f.m_proxyCount) { + val proxy = f.m_proxies!![i] + val aabb = m_contactManager.m_broadPhase.getFatAABB(proxy.proxyId) + if (aabb != null) { + val vs = avs[4] + vs[0].set(aabb.lowerBound.x, aabb.lowerBound.y) + vs[1].set(aabb.upperBound.x, aabb.lowerBound.y) + vs[2].set(aabb.upperBound.x, aabb.upperBound.y) + vs[3].set(aabb.lowerBound.x, aabb.upperBound.y) + m_debugDraw!!.drawPolygon(vs, 4, color) + } + } + f = f.getNext() + } + b = b.getNext() + } + } + + if (flags and DebugDraw.e_centerOfMassBit != 0) { + var b = bodyList + while (b != null) { + xf.set(b.transform) + xf.p.set(b.worldCenter) + m_debugDraw!!.drawTransform(xf) + b = b.getNext() + } + } + + if (flags and DebugDraw.e_dynamicTreeBit != 0) { + m_contactManager.m_broadPhase.drawTree(m_debugDraw!!) + } + + m_debugDraw!!.flush() + } + + /** + * Query the world for all fixtures that potentially overlap the provided AABB. + * + * @param callback a user implemented callback class. + * @param aabb the query box. + */ + fun queryAABB(callback: QueryCallback, aabb: AABB) { + wqwrapper.broadPhase = m_contactManager.m_broadPhase + wqwrapper.callback = callback + m_contactManager.m_broadPhase.query(wqwrapper, aabb) + } + + /** + * Query the world for all fixtures and particles that potentially overlap the provided AABB. + * + * @param callback a user implemented callback class. + * @param particleCallback callback for particles. + * @param aabb the query box. + */ + fun queryAABB(callback: QueryCallback, particleCallback: ParticleQueryCallback, aabb: AABB) { + wqwrapper.broadPhase = m_contactManager.m_broadPhase + wqwrapper.callback = callback + m_contactManager.m_broadPhase.query(wqwrapper, aabb) + m_particleSystem.queryAABB(particleCallback, aabb) + } + + /** + * Query the world for all particles that potentially overlap the provided AABB. + * + * @param particleCallback callback for particles. + * @param aabb the query box. + */ + fun queryAABB(particleCallback: ParticleQueryCallback, aabb: AABB) { + m_particleSystem.queryAABB(particleCallback, aabb) + } + + /** + * Ray-cast the world for all fixtures in the path of the ray. Your callback controls whether you + * get the closest point, any point, or n-points. The ray-cast ignores shapes that contain the + * starting point. + * + * @param callback a user implemented callback class. + * @param point1 the ray starting point + * @param point2 the ray ending point + */ + fun raycast(callback: RayCastCallback, point1: Vec2, point2: Vec2) { + wrcwrapper.broadPhase = m_contactManager.m_broadPhase + wrcwrapper.callback = callback + input.maxFraction = 1.0f + input.p1.set(point1) + input.p2.set(point2) + m_contactManager.m_broadPhase.raycast(wrcwrapper, input) + } + + /** + * Ray-cast the world for all fixtures and particles in the path of the ray. Your callback + * controls whether you get the closest point, any point, or n-points. The ray-cast ignores shapes + * that contain the starting point. + * + * @param callback a user implemented callback class. + * @param particleCallback the particle callback class. + * @param point1 the ray starting point + * @param point2 the ray ending point + */ + fun raycast(callback: RayCastCallback, particleCallback: ParticleRaycastCallback, + point1: Vec2, point2: Vec2) { + wrcwrapper.broadPhase = m_contactManager.m_broadPhase + wrcwrapper.callback = callback + input.maxFraction = 1.0f + input.p1.set(point1) + input.p2.set(point2) + m_contactManager.m_broadPhase.raycast(wrcwrapper, input) + m_particleSystem.raycast(particleCallback, point1, point2) + } + + /** + * Ray-cast the world for all particles in the path of the ray. Your callback controls whether you + * get the closest point, any point, or n-points. + * + * @param particleCallback the particle callback class. + * @param point1 the ray starting point + * @param point2 the ray ending point + */ + fun raycast(particleCallback: ParticleRaycastCallback, point1: Vec2, point2: Vec2) { + m_particleSystem.raycast(particleCallback, point1, point2) + } + + private fun solve(step: TimeStep) { + profile.solveInit.startAccum() + profile.solveVelocity.startAccum() + profile.solvePosition.startAccum() + + // update previous transforms + run { + var b = bodyList + while (b != null) { + b!!.xf0.set(b!!.xf) + b = b!!.m_next + } + } + + // Size the island for the worst case. + island.init(bodyCount, m_contactManager.m_contactCount, jointCount, m_contactManager.m_contactListener) + + // Clear all the island flags. + run { + var b = bodyList + while (b != null) { + b!!.flags = b!!.flags and Body.e_islandFlag.inv() + b = b!!.m_next + } + } + var c: Contact? = m_contactManager.m_contactList + while (c != null) { + c.m_flags = c.m_flags and Contact.ISLAND_FLAG.inv() + c = c.m_next + } + var j = jointList + while (j != null) { + j.islandFlag = false + j = j.next + } + + // Build and simulate all awake islands. + val stackSize = bodyCount + if (stack.size < stackSize) { + stack = arrayOfNulls(stackSize) + } + var seed = bodyList + while (seed != null) { + if (seed.flags and Body.e_islandFlag == Body.e_islandFlag) { + seed = seed.m_next + continue + } + + if (!seed.isAwake || !seed.isActive) { + seed = seed.m_next + continue + } + + // The seed can be dynamic or kinematic. + if (seed.type === BodyType.STATIC) { + seed = seed.m_next + continue + } + + // Reset island and stack. + island.clear() + var stackCount = 0 + stack[stackCount++] = seed + seed.flags = seed.flags or Body.e_islandFlag + + // Perform a depth first search (DFS) on the constraint graph. + while (stackCount > 0) { + // Grab the next body off the stack and add it to the island. + val b = stack[--stackCount]!! + assert(b.isActive) + island.add(b) + + // Make sure the body is awake. + b.isAwake = true + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b.type === BodyType.STATIC) { + continue + } + + // Search all contacts connected to this body. + var ce = b.m_contactList + while (ce != null) { + val contact = ce.contact + + // Has this contact already been added to an island? + if (contact!!.m_flags and Contact.ISLAND_FLAG == Contact.ISLAND_FLAG) { + ce = ce.next + continue + } + + // Is this contact solid and touching? + if (!contact.isEnabled || !contact.isTouching) { + ce = ce.next + continue + } + + // Skip sensors. + val sensorA = contact.m_fixtureA!!.m_isSensor + val sensorB = contact.m_fixtureB!!.m_isSensor + if (sensorA || sensorB) { + ce = ce.next + continue + } + + island.add(contact) + contact.m_flags = contact.m_flags or Contact.ISLAND_FLAG + + val other = ce.other + + // Was the other body already added to this island? + if (other!!.flags and Body.e_islandFlag == Body.e_islandFlag) { + ce = ce.next + continue + } + + assert(stackCount < stackSize) + stack[stackCount++] = other + other.flags = other.flags or Body.e_islandFlag + ce = ce.next + } + + // Search all joints connect to this body. + var je = b.m_jointList + while (je != null) { + if (je.joint!!.islandFlag) { + je = je.next + continue + } + + val other = je.other + + // Don't simulate joints connected to inactive bodies. + if (!other!!.isActive) { + je = je.next + continue + } + + island.add(je.joint!!) + je.joint!!.islandFlag = true + + if (other.flags and Body.e_islandFlag == Body.e_islandFlag) { + je = je.next + continue + } + + assert(stackCount < stackSize) + stack[stackCount++] = other + other.flags = other.flags or Body.e_islandFlag + je = je.next + } + } + island.solve(profile, step, gravity, isSleepingAllowed) + + // Post solve cleanup. + for (i in 0 until island.m_bodyCount) { + // Allow static bodies to participate in other islands. + val b = island.m_bodies!![i] + if (b.type === BodyType.STATIC) { + b.flags = b.flags and Body.e_islandFlag.inv() + } + } + seed = seed.m_next + } + profile.solveInit.endAccum() + profile.solveVelocity.endAccum() + profile.solvePosition.endAccum() + + broadphaseTimer.reset() + // Synchronize fixtures, check for out of range bodies. + var b = bodyList + while (b != null) { + // If a body was not in an island then it did not move. + if (b!!.flags and Body.e_islandFlag == 0) { + b = b!!.getNext() + continue + } + + if (b!!.type === BodyType.STATIC) { + b = b!!.getNext() + continue + } + + // Update fixtures (for broad-phase). + b!!.synchronizeFixtures() + b = b!!.getNext() + } + + // Look for new contacts. + m_contactManager.findNewContacts() + profile.broadphase.record(broadphaseTimer.milliseconds) + } + + private fun solveTOI(step: TimeStep) { + + val island = toiIsland + island.init(2 * Settings.maxTOIContacts, Settings.maxTOIContacts, 0, m_contactManager.m_contactListener) + if (m_stepComplete) { + var b = bodyList + while (b != null) { + b.flags = b.flags and Body.e_islandFlag.inv() + b.sweep.alpha0 = 0.0f + b = b.m_next + } + + var c: Contact? = m_contactManager.m_contactList + while (c != null) { + // Invalidate TOI + c.m_flags = c.m_flags and (Contact.TOI_FLAG or Contact.ISLAND_FLAG).inv() + c.m_toiCount = 0f + c.m_toi = 1.0f + c = c.m_next + } + } + + // Find TOI events and solve them. + while (true) { + // Find the first TOI. + var minContact: Contact? = null + var minAlpha = 1.0f + + var c: Contact? = m_contactManager.m_contactList + while (c != null) { + // Is this contact disabled? + if (!c.isEnabled) { + c = c.m_next + continue + } + + // Prevent excessive sub-stepping. + if (c.m_toiCount > Settings.maxSubSteps) { + c = c.m_next + continue + } + + var alpha = 1.0f + if (c.m_flags and Contact.TOI_FLAG != 0) { + // This contact has a valid cached TOI. + alpha = c.m_toi + } else { + val fA = c.getFixtureA() + val fB = c.getFixtureB() + + // Is there a sensor? + if (fA!!.isSensor || fB!!.isSensor) { + c = c.m_next + continue + } + + val bA = fA.getBody() + val bB = fB.getBody() + + val typeA = bA!!._type + val typeB = bB!!._type + assert(typeA === BodyType.DYNAMIC || typeB === BodyType.DYNAMIC) + + val activeA = bA.isAwake && typeA !== BodyType.STATIC + val activeB = bB.isAwake && typeB !== BodyType.STATIC + + // Is at least one body active (awake and dynamic or kinematic)? + if (!activeA && !activeB) { + c = c.m_next + continue + } + + val collideA = bA.isBullet || typeA !== BodyType.DYNAMIC + val collideB = bB.isBullet || typeB !== BodyType.DYNAMIC + + // Are these two non-bullet dynamic bodies? + if (!collideA && !collideB) { + c = c.m_next + continue + } + + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + var alpha0 = bA.sweep.alpha0 + + if (bA.sweep.alpha0 < bB.sweep.alpha0) { + alpha0 = bB.sweep.alpha0 + bA.sweep.advance(alpha0) + } else if (bB.sweep.alpha0 < bA.sweep.alpha0) { + alpha0 = bA.sweep.alpha0 + bB.sweep.advance(alpha0) + } + + assert(alpha0 < 1.0f) + + val indexA = c.getChildIndexA() + val indexB = c.getChildIndexB() + + // Compute the time of impact in interval [0, minTOI] + val input = toiInput + input.proxyA.set(fA.getShape()!!, indexA) + input.proxyB.set(fB.getShape()!!, indexB) + input.sweepA.set(bA.sweep) + input.sweepB.set(bB.sweep) + input.tMax = 1.0f + + pool.timeOfImpact.timeOfImpact(toiOutput, input) + + // Beta is the fraction of the remaining portion of the . + val beta = toiOutput.t + if (toiOutput.state === TimeOfImpact.TOIOutputState.TOUCHING) { + alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f) + } else { + alpha = 1.0f + } + + c.m_toi = alpha + c.m_flags = c.m_flags or Contact.TOI_FLAG + } + + if (alpha < minAlpha) { + // This is the minimum TOI found so far. + minContact = c + minAlpha = alpha + } + c = c.m_next + } + + if (minContact == null || 1.0f - 10.0f * Settings.EPSILON < minAlpha) { + // No more TOI events. Done! + m_stepComplete = true + break + } + + // Advance the bodies to the TOI. + val fA = minContact.getFixtureA() + val fB = minContact.getFixtureB() + val bA = fA!!.getBody() + val bB = fB!!.getBody() + + backup1.set(bA!!.sweep) + backup2.set(bB!!.sweep) + + bA.advance(minAlpha) + bB.advance(minAlpha) + + // The TOI contact likely has some new contact points. + minContact.update(m_contactManager.m_contactListener) + minContact.m_flags = minContact.m_flags and Contact.TOI_FLAG.inv() + ++minContact.m_toiCount + + // Is the contact solid? + if (!minContact.isEnabled || !minContact.isTouching) { + // Restore the sweeps. + minContact.isEnabled = false + bA.sweep.set(backup1) + bB.sweep.set(backup2) + bA.synchronizeTransform() + bB.synchronizeTransform() + continue + } + + bA.isAwake = true + bB.isAwake = true + + // Build the island + island.clear() + island.add(bA) + island.add(bB) + island.add(minContact) + + bA.flags = bA.flags or Body.e_islandFlag + bB.flags = bB.flags or Body.e_islandFlag + minContact.m_flags = minContact.m_flags or Contact.ISLAND_FLAG + + // Get contacts on bodyA and bodyB. + tempBodies[0] = bA + tempBodies[1] = bB + for (i in 0..1) { + val body = tempBodies[i]!! + if (body._type === BodyType.DYNAMIC) { + var ce = body.m_contactList + while (ce != null) { + if (island.m_bodyCount == island.m_bodyCapacity) { + break + } + + if (island.m_contactCount == island.m_contactCapacity) { + break + } + + val contact = ce.contact + + // Has this contact already been added to the island? + if (contact!!.m_flags and Contact.ISLAND_FLAG != 0) { + ce = ce.next + continue + } + + // Only add static, kinematic, or bullet bodies. + val other = ce.other + if (other!!._type === BodyType.DYNAMIC && !body.isBullet && !other!!.isBullet) { + ce = ce.next + continue + } + + // Skip sensors. + val sensorA = contact.m_fixtureA!!.m_isSensor + val sensorB = contact.m_fixtureB!!.m_isSensor + if (sensorA || sensorB) { + ce = ce.next + continue + } + + // Tentatively advance the body to the TOI. + backup1.set(other!!.sweep) + if (other.flags and Body.e_islandFlag == 0) { + other.advance(minAlpha) + } + + // Update the contact points + contact.update(m_contactManager.m_contactListener) + + // Was the contact disabled by the user? + if (!contact.isEnabled) { + other.sweep.set(backup1) + other.synchronizeTransform() + ce = ce.next + continue + } + + // Are there contact points? + if (!contact.isTouching) { + other.sweep.set(backup1) + other.synchronizeTransform() + ce = ce.next + continue + } + + // Add the contact to the island + contact.m_flags = contact.m_flags or Contact.ISLAND_FLAG + island.add(contact) + + // Has the other body already been added to the island? + if (other.flags and Body.e_islandFlag != 0) { + ce = ce.next + continue + } + + // Add the other body to the island. + other.flags = other.flags or Body.e_islandFlag + + if (other._type !== BodyType.STATIC) { + other.isAwake = true + } + + island.add(other) + ce = ce.next + } + } + } + + subStep.dt = (1.0f - minAlpha) * step.dt + subStep.inv_dt = 1.0f / subStep.dt + subStep.dtRatio = 1.0f + subStep.positionIterations = 20 + subStep.velocityIterations = step.velocityIterations + subStep.warmStarting = false + island.solveTOI(subStep, bA.islandIndex, bB.islandIndex) + + // Reset island flags and synchronize broad-phase proxies. + for (i in 0 until island.m_bodyCount) { + val body = island.m_bodies!![i] + body.flags = body.flags and Body.e_islandFlag.inv() + + if (body._type !== BodyType.DYNAMIC) { + continue + } + + body.synchronizeFixtures() + + // Invalidate all contact TOIs on this displaced body. + var ce = body.m_contactList + while (ce != null) { + ce.contact!!.m_flags = ce.contact!!.m_flags and (Contact.TOI_FLAG or Contact.ISLAND_FLAG).inv() + ce = ce.next + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts are created. + // Also, some contacts can be destroyed. + m_contactManager.findNewContacts() + + if (isSubStepping) { + m_stepComplete = false + break + } + } + } + + private fun drawJoint(joint: Joint) { + val bodyA = joint.bodyA + val bodyB = joint.bodyB + val xf1 = bodyA!!.transform + val xf2 = bodyB!!.transform + val x1 = xf1.p + val x2 = xf2.p + val p1 = pool.popVec2() + val p2 = pool.popVec2() + joint.getAnchorA(p1) + joint.getAnchorB(p2) + + color.set(0.5f, 0.8f, 0.8f) + + when (joint.type) { + // TODO djm write after writing joints + JointType.DISTANCE -> m_debugDraw!!.drawSegment(p1, p2, color) + + JointType.PULLEY -> { + val pulley = joint as PulleyJoint + val s1 = pulley.getGroundAnchorA() + val s2 = pulley.getGroundAnchorB() + m_debugDraw!!.drawSegment(s1, p1, color) + m_debugDraw!!.drawSegment(s2, p2, color) + m_debugDraw!!.drawSegment(s1, s2, color) + } + JointType.CONSTANT_VOLUME, JointType.MOUSE -> { + } + else -> { + m_debugDraw!!.drawSegment(x1, p1, color) + m_debugDraw!!.drawSegment(p1, p2, color) + m_debugDraw!!.drawSegment(x2, p2, color) + } + }// don't draw this + pool.pushVec2(2) + } + + private fun drawShape(fixture: Fixture, xf: Transform, color: Color3f, wireframe: Boolean) { + when (fixture.type) { + ShapeType.CIRCLE -> { + val circle = fixture.getShape() as CircleShape? + + // Vec2 center = Mul(xf, circle.m_p); + Transform.mulToOutUnsafe(xf, circle!!.p, center) + val radius = circle.radius + xf.q.getXAxis(axis) + + if (fixture.userData != null && fixture.userData == LIQUID_INT) { + val b = fixture.getBody() + liquidOffset.set(b!!._linearVelocity) + val linVelLength = b._linearVelocity.length() + if (averageLinearVel == -1f) { + averageLinearVel = linVelLength + } else { + averageLinearVel = .98f * averageLinearVel + .02f * linVelLength + } + liquidOffset.mulLocal(liquidLength / averageLinearVel / 2f) + circCenterMoved.set(center).addLocal(liquidOffset) + center.subLocal(liquidOffset) + m_debugDraw!!.drawSegment(center, circCenterMoved, liquidColor) + return + } + if (wireframe) { + m_debugDraw!!.drawCircle(center, radius, axis, color) + } else { + m_debugDraw!!.drawSolidCircle(center, radius, axis, color) + } + } + + ShapeType.POLYGON -> { + val poly = fixture.getShape() as PolygonShape? + val vertexCount = poly!!.count + assert(vertexCount <= Settings.maxPolygonVertices) + val vertices = tlvertices[Settings.maxPolygonVertices] + + for (i in 0 until vertexCount) { + // vertices[i] = Mul(xf, poly.m_vertices[i]); + Transform.mulToOutUnsafe(xf, poly.vertices[i], vertices[i]) + } + if (wireframe) { + m_debugDraw!!.drawPolygon(vertices, vertexCount, color) + } else { + m_debugDraw!!.drawSolidPolygon(vertices, vertexCount, color) + } + } + ShapeType.EDGE -> { + val edge = fixture.getShape() as EdgeShape? + Transform.mulToOutUnsafe(xf, edge!!.vertex1, v1) + Transform.mulToOutUnsafe(xf, edge.vertex2, v2) + m_debugDraw!!.drawSegment(v1, v2, color) + } + ShapeType.CHAIN -> { + val chain = fixture.getShape() as ChainShape? + val count = chain!!.count + val vertices = chain.vertices + + Transform.mulToOutUnsafe(xf, vertices!![0], v1) + for (i in 1 until count) { + Transform.mulToOutUnsafe(xf, vertices[i], v2) + m_debugDraw!!.drawSegment(v1, v2, color) + m_debugDraw!!.drawCircle(v1, 0.05f, color) + v1.set(v2) + } + } + else -> { + } + } + } + + private fun drawParticleSystem(system: ParticleSystem) { + val wireframe = m_debugDraw!!.flags and DebugDraw.e_wireframeDrawingBit != 0 + val particleCount = system.particleCount + if (particleCount != 0) { + val particleRadius = system.particleRadius + val positionBuffer = system.particlePositionBuffer + var colorBuffer: Array? = null + if (system.m_colorBuffer.data != null) { + colorBuffer = system.particleColorBuffer + } + if (wireframe) { + m_debugDraw!!.drawParticlesWireframe(positionBuffer!!, particleRadius, colorBuffer!!, particleCount) + } else { + m_debugDraw!!.drawParticles(positionBuffer!!, particleRadius, colorBuffer!!, particleCount) + } + } + } + + /** + * Create a particle whose properties have been defined. No reference to the definition is + * retained. A simulation step must occur before it's possible to interact with a newly created + * particle. For example, DestroyParticleInShape() will not destroy a particle until Step() has + * been called. + * + * @warning This function is locked during callbacks. + * @return the index of the particle. + */ + fun createParticle(def: ParticleDef): Int { + assert(!isLocked) + if (isLocked) { + return 0 + } + val p = m_particleSystem.createParticle(def) + return p + } + + /** + * Destroy a particle. The particle is removed after the next step. + * + * @param Index of the particle to destroy. + * @param Whether to call the destruction listener just before the particle is destroyed. + */ + + fun destroyParticle(index: Int, callDestructionListener: Boolean = false) { + m_particleSystem.destroyParticle(index, callDestructionListener) + } + + /** + * Destroy particles inside a shape. This function is locked during callbacks. In addition, this + * function immediately destroys particles in the shape in contrast to DestroyParticle() which + * defers the destruction until the next simulation step. + * + * @param Shape which encloses particles that should be destroyed. + * @param Transform applied to the shape. + * @param Whether to call the world b2DestructionListener for each particle destroyed. + * @warning This function is locked during callbacks. + * @return Number of particles destroyed. + */ + + fun destroyParticlesInShape(shape: Shape, xf: Transform, callDestructionListener: Boolean = false): Int { + assert(!isLocked) + return if (isLocked) { + 0 + } else m_particleSystem.destroyParticlesInShape(shape, xf, callDestructionListener) + } + + /** + * Create a particle group whose properties have been defined. No reference to the definition is + * retained. + * + * @warning This function is locked during callbacks. + */ + fun createParticleGroup(def: ParticleGroupDef): ParticleGroup? { + assert(!isLocked) + if (isLocked) { + return null + } + val g = m_particleSystem.createParticleGroup(def) + return g + } + + /** + * Join two particle groups. + * + * @param the first group. Expands to encompass the second group. + * @param the second group. It is destroyed. + * @warning This function is locked during callbacks. + */ + fun joinParticleGroups(groupA: ParticleGroup, groupB: ParticleGroup) { + assert(!isLocked) + if (isLocked) { + return + } + m_particleSystem.joinParticleGroups(groupA, groupB) + } + + /** + * Destroy particles in a group. This function is locked during callbacks. + * + * @param The particle group to destroy. + * @param Whether to call the world b2DestructionListener for each particle is destroyed. + * @warning This function is locked during callbacks. + */ + + fun destroyParticlesInGroup(group: ParticleGroup, callDestructionListener: Boolean = false) { + assert(!isLocked) + if (isLocked) { + return + } + m_particleSystem.destroyParticlesInGroup(group, callDestructionListener) + } + + /** + * Set a buffer for particle data. + * + * @param buffer is a pointer to a block of memory. + * @param size is the number of values in the block. + */ + fun setParticleFlagsBuffer(buffer: IntArray, capacity: Int) { + m_particleSystem.setParticleFlagsBuffer(buffer, capacity) + } + + fun setParticlePositionBuffer(buffer: Array, capacity: Int) { + m_particleSystem.setParticlePositionBuffer(buffer, capacity) + + } + + fun setParticleVelocityBuffer(buffer: Array, capacity: Int) { + m_particleSystem.setParticleVelocityBuffer(buffer, capacity) + + } + + fun setParticleColorBuffer(buffer: Array, capacity: Int) { + m_particleSystem.setParticleColorBuffer(buffer, capacity) + + } + + fun setParticleUserDataBuffer(buffer: Array, capacity: Int) { + m_particleSystem.setParticleUserDataBuffer(buffer, capacity) + } + + /** + * Compute the kinetic energy that can be lost by damping force + * + * @return + */ + fun computeParticleCollisionEnergy(): Float { + return m_particleSystem.computeParticleCollisionEnergy() + } + + companion object { + val WORLD_POOL_SIZE = 100 + val WORLD_POOL_CONTAINER_SIZE = 10 + + val NEW_FIXTURE = 0x0001 + val LOCKED = 0x0002 + val CLEAR_FORCES = 0x0004 + + // NOTE this corresponds to the liquid test, so the debugdraw can draw + // the liquid particles correctly. They should be the same. + private val LIQUID_INT = 1234598372 + } +} +/** + * Construct a world object. + * + * @param gravity the world gravity vector. + */ +/** + * Construct a world object. + * + * @param gravity the world gravity vector. + */ +/** + * Destroy a particle. The particle is removed after the next step. + * + * @param index + */ +/** + * Destroy particles inside a shape without enabling the destruction callback for destroyed + * particles. This function is locked during callbacks. For more information see + * DestroyParticleInShape(Shape&, Transform&,bool). + * + * @param Shape which encloses particles that should be destroyed. + * @param Transform applied to the shape. + * @warning This function is locked during callbacks. + * @return Number of particles destroyed. + */ +/** + * Destroy particles in a group without enabling the destruction callback for destroyed particles. + * This function is locked during callbacks. + * + * @param The particle group to destroy. + * @warning This function is locked during callbacks. + */ + + +internal class WorldQueryWrapper : TreeCallback { + + var broadPhase: BroadPhase? = null + var callback: QueryCallback? = null + override fun treeCallback(nodeId: Int): Boolean { + val proxy = broadPhase!!.getUserData(nodeId) as FixtureProxy? + return callback!!.reportFixture(proxy!!.fixture!!) + } +} + + +internal class WorldRayCastWrapper : TreeRayCastCallback { + + // djm pooling + private val output = RayCastOutput() + private val temp = Vec2() + private val point = Vec2() + + var broadPhase: BroadPhase? = null + var callback: RayCastCallback? = null + + override fun raycastCallback(input: RayCastInput, nodeId: Int): Float { + val userData = broadPhase!!.getUserData(nodeId) + val proxy = userData as FixtureProxy? + val fixture = proxy!!.fixture + val index = proxy.childIndex + val hit = fixture!!.raycast(output, input, index) + + if (hit) { + val fraction = output.fraction + // Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; + temp.set(input.p2).mulLocal(fraction) + point.set(input.p1).mulLocal(1 - fraction).addLocal(temp) + return callback!!.reportFixture(fixture, point, output.normal, fraction) + } + + return input.maxFraction + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRef.kt new file mode 100644 index 0000000..ada40f6 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRef.kt @@ -0,0 +1,5 @@ +package org.jbox2d.dynamics + +interface WorldRef { + val world: World +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRefExt.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRefExt.kt new file mode 100644 index 0000000..dd0e0c1 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/WorldRefExt.kt @@ -0,0 +1,19 @@ +package org.jbox2d.dynamics + +import org.jbox2d.dynamics.joints.Joint + +inline fun WorldRef.forEachBody(callback: (body: Body) -> Unit) { + var node = world.bodyList + while (node != null) { + callback(node) + node = node.m_next + } +} + +inline fun WorldRef.forEachJoint(callback: (joint: Joint) -> Unit) { + var node = world.jointList + while (node != null) { + callback(node) + node = node.next + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndCircleContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndCircleContact.kt new file mode 100644 index 0000000..fb5bf6e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndCircleContact.kt @@ -0,0 +1,52 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.ChainShape +import org.jbox2d.collision.shapes.CircleShape +import org.jbox2d.collision.shapes.EdgeShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class ChainAndCircleContact(argPool: IWorldPool) : Contact(argPool) { + + private val edge = EdgeShape() + + override fun init(fA: Fixture, indexA: Int, fB: Fixture, indexB: Int) { + super.init(fA, indexA, fB, indexB) + assert(m_fixtureA!!.type === ShapeType.CHAIN) + assert(m_fixtureB!!.type === ShapeType.CIRCLE) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + val chain = m_fixtureA!!.m_shape as ChainShape + chain.getChildEdge(edge, m_indexA) + pool.collision.collideEdgeAndCircle(manifold, edge, xfA, + m_fixtureB!!.m_shape as CircleShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndPolygonContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndPolygonContact.kt new file mode 100644 index 0000000..ca874cb --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ChainAndPolygonContact.kt @@ -0,0 +1,52 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.ChainShape +import org.jbox2d.collision.shapes.EdgeShape +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class ChainAndPolygonContact(argPool: IWorldPool) : Contact(argPool) { + + private val edge = EdgeShape() + + override fun init(fA: Fixture, indexA: Int, fB: Fixture, indexB: Int) { + super.init(fA, indexA, fB, indexB) + assert(m_fixtureA!!.type === ShapeType.CHAIN) + assert(m_fixtureB!!.type === ShapeType.POLYGON) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + val chain = m_fixtureA!!.m_shape as ChainShape + chain.getChildEdge(edge, m_indexA) + pool.collision.collideEdgeAndPolygon(manifold, edge, xfA, + m_fixtureB!!.m_shape as PolygonShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/CircleContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/CircleContact.kt new file mode 100644 index 0000000..f522e05 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/CircleContact.kt @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.CircleShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class CircleContact(argPool: IWorldPool) : Contact(argPool) { + + fun init(fixtureA: Fixture, fixtureB: Fixture) { + super.init(fixtureA, 0, fixtureB, 0) + assert(m_fixtureA!!.type === ShapeType.CIRCLE) + assert(m_fixtureB!!.type === ShapeType.CIRCLE) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + pool.collision.collideCircles(manifold, m_fixtureA!!.m_shape as CircleShape, xfA, + m_fixtureB!!.m_shape as CircleShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Contact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Contact.kt new file mode 100644 index 0000000..cf6d4e6 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Contact.kt @@ -0,0 +1,333 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + + +import org.jbox2d.callbacks.ContactListener +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.WorldManifold +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.pooling.IWorldPool + +/** + * The class manages contact between two shapes. A contact exists for each overlapping AABB in the + * broad-phase (except if filtered). Therefore a contact object may exist that has no contact + * points. + * + * @author daniel + */ +abstract class Contact protected constructor( protected val pool: IWorldPool) { + + var m_flags: Int = 0 + + // World pool and list pointers. + + var m_prev: Contact? = null + /** + * Get the next contact in the world's contact list. + * + * @return + */ + + var m_next: Contact? = null + + fun getPrev() = m_prev + fun getNext() = m_next + + // Nodes for connecting bodies. + + var m_nodeA: ContactEdge = ContactEdge() + + var m_nodeB: ContactEdge = ContactEdge() + + /** + * Get the first fixture in this contact. + * + * @return + */ + + var m_fixtureA: Fixture? = null + /** + * Get the second fixture in this contact. + * + * @return + */ + + var m_fixtureB: Fixture? = null + + fun getFixtureA() = m_fixtureA + fun getFixtureB() = m_fixtureB + + + var m_indexA: Int = 0 + + var m_indexB: Int = 0 + + fun getChildIndexA() = m_indexA + fun getChildIndexB() = m_indexB + + /** + * Get the contact manifold. Do not set the point count to zero. Instead call Disable. + */ + + val m_manifold: Manifold = Manifold() + + fun getManifold(): Manifold = m_manifold + + + var m_toiCount: Float = 0.toFloat() + + var m_toi: Float = 0.toFloat() + + + var m_friction: Float = 0.toFloat() + + var m_restitution: Float = 0.toFloat() + + + var m_tangentSpeed: Float = 0.toFloat() + + /** + * Is this contact touching + * + * @return + */ + val isTouching: Boolean + get() = m_flags and TOUCHING_FLAG == TOUCHING_FLAG + + /** + * Has this contact been disabled? + * + * @return + */ + /** + * Enable/disable this contact. This can be used inside the pre-solve contact listener. The + * contact is only disabled for the current time step (or sub-step in continuous collisions). + * + * @param flag + */ + var isEnabled: Boolean + get() = m_flags and ENABLED_FLAG == ENABLED_FLAG + set(flag) = if (flag) { + m_flags = m_flags or ENABLED_FLAG + } else { + m_flags = m_flags and ENABLED_FLAG.inv() + } + + // djm pooling + private val oldManifold = Manifold() + + /** initialization for pooling */ + open fun init(fA: Fixture, indexA: Int, fB: Fixture, indexB: Int) { + m_flags = ENABLED_FLAG + + m_fixtureA = fA + m_fixtureB = fB + + m_indexA = indexA + m_indexB = indexB + + m_manifold.pointCount = 0 + + m_prev = null + m_next = null + + m_nodeA!!.contact = null + m_nodeA!!.prev = null + m_nodeA!!.next = null + m_nodeA!!.other = null + + m_nodeB!!.contact = null + m_nodeB!!.prev = null + m_nodeB!!.next = null + m_nodeB!!.other = null + + m_toiCount = 0f + m_friction = Contact.mixFriction(fA.m_friction, fB.m_friction) + m_restitution = Contact.mixRestitution(fA.m_restitution, fB.m_restitution) + + m_tangentSpeed = 0f + } + + /** + * Get the world manifold. + */ + fun getWorldManifold(worldManifold: WorldManifold) { + val bodyA = m_fixtureA!!.m_body + val bodyB = m_fixtureB!!.m_body + val shapeA = m_fixtureA!!.m_shape + val shapeB = m_fixtureB!!.m_shape + + worldManifold.initialize(m_manifold, bodyA!!.xf, shapeA!!.radius, + bodyB!!.xf, shapeB!!.radius) + } + + fun resetFriction() { + m_friction = Contact.mixFriction(m_fixtureA!!.m_friction, m_fixtureB!!.m_friction) + } + + fun resetRestitution() { + m_restitution = Contact.mixRestitution(m_fixtureA!!.m_restitution, m_fixtureB!!.m_restitution) + } + + abstract fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) + + /** + * Flag this contact for filtering. Filtering will occur the next time step. + */ + fun flagForFiltering() { + m_flags = m_flags or FILTER_FLAG + } + + fun update(listener: ContactListener?) { + + oldManifold.set(m_manifold) + + // Re-enable this contact. + m_flags = m_flags or ENABLED_FLAG + + var touching = false + val wasTouching = m_flags and TOUCHING_FLAG == TOUCHING_FLAG + + val sensorA = m_fixtureA!!.isSensor + val sensorB = m_fixtureB!!.isSensor + val sensor = sensorA || sensorB + + val bodyA = m_fixtureA!!.m_body!! + val bodyB = m_fixtureB!!.m_body!! + val xfA = bodyA.xf + val xfB = bodyB.xf + // log.debug("TransformA: "+xfA); + // log.debug("TransformB: "+xfB); + + if (sensor) { + val shapeA = m_fixtureA!!.m_shape!! + val shapeB = m_fixtureB!!.m_shape!! + touching = pool.collision.testOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB) + + // Sensors don't generate manifolds. + m_manifold.pointCount = 0 + } else { + evaluate(m_manifold, xfA, xfB) + touching = m_manifold.pointCount > 0 + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (i in 0 until m_manifold.pointCount) { + val mp2 = m_manifold.points[i] + mp2.normalImpulse = 0.0f + mp2.tangentImpulse = 0.0f + val id2 = mp2.id + + for (j in 0 until oldManifold.pointCount) { + val mp1 = oldManifold.points[j] + + if (mp1.id.isEqual(id2)) { + mp2.normalImpulse = mp1.normalImpulse + mp2.tangentImpulse = mp1.tangentImpulse + break + } + } + } + + if (touching != wasTouching) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + } + } + + if (touching) { + m_flags = m_flags or TOUCHING_FLAG + } else { + m_flags = m_flags and TOUCHING_FLAG.inv() + } + + if (listener == null) { + return + } + + if (!wasTouching && touching) { + listener.beginContact(this) + } + + if (wasTouching && !touching) { + listener.endContact(this) + } + + if (!sensor && touching) { + listener.preSolve(this, oldManifold) + } + } + + companion object { + + // Flags stored in m_flags + // Used when crawling contact graph when forming islands. + + val ISLAND_FLAG = 0x0001 + // Set when the shapes are touching. + + val TOUCHING_FLAG = 0x0002 + // This contact can be disabled (by user) + + val ENABLED_FLAG = 0x0004 + // This contact needs filtering because a fixture filter was changed. + + val FILTER_FLAG = 0x0008 + // This bullet contact had a TOI event + + val BULLET_HIT_FLAG = 0x0010 + + + val TOI_FLAG = 0x0020 + + /** + * Friction mixing law. The idea is to allow either fixture to drive the restitution to zero. For + * example, anything slides on ice. + * + * @param friction1 + * @param friction2 + * @return + */ + + fun mixFriction(friction1: Float, friction2: Float): Float { + return MathUtils.sqrt(friction1 * friction2) + } + + /** + * Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. For + * example, a superball bounces on anything. + * + * @param restitution1 + * @param restitution2 + * @return + */ + + fun mixRestitution(restitution1: Float, restitution2: Float): Float { + return if (restitution1 > restitution2) restitution1 else restitution2 + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactCreator.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactCreator.kt new file mode 100644 index 0000000..40cb771 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactCreator.kt @@ -0,0 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.dynamics.Fixture +import org.jbox2d.pooling.IWorldPool + +// updated to rev 100 +interface ContactCreator { + + fun contactCreateFcn(argPool: IWorldPool, fixtureA: Fixture, fixtureB: Fixture): Contact + + fun contactDestroyFcn(argPool: IWorldPool, contact: Contact) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactEdge.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactEdge.kt new file mode 100644 index 0000000..77781ea --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactEdge.kt @@ -0,0 +1,60 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.dynamics.Body + +/** + * A contact edge is used to connect bodies and contacts together in a contact graph where each body + * is a node and each contact is an edge. A contact edge belongs to a doubly linked list maintained + * in each attached body. Each contact has two contact nodes, one for each attached body. + * + * @author daniel + */ +class ContactEdge { + + /** + * provides quick access to the other body attached. + */ + + var other: Body? = null + + /** + * the contact + */ + + var contact: Contact? = null + + /** + * the previous contact edge in the body's contact list + */ + + var prev: ContactEdge? = null + + /** + * the next contact edge in the body's contact list + */ + + var next: ContactEdge? = null +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactPositionConstraint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactPositionConstraint.kt new file mode 100644 index 0000000..d3ecc00 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactPositionConstraint.kt @@ -0,0 +1,61 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold.ManifoldType +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 + +class ContactPositionConstraint { + + internal var localPoints = Array(Settings.maxManifoldPoints) { Vec2() } + + internal val localNormal = Vec2() + + internal val localPoint = Vec2() + + internal var indexA: Int = 0 + + internal var indexB: Int = 0 + + internal var invMassA: Float = 0.toFloat() + + internal var invMassB: Float = 0.toFloat() + + internal val localCenterA = Vec2() + + internal val localCenterB = Vec2() + + internal var invIA: Float = 0.toFloat() + + internal var invIB: Float = 0.toFloat() + + internal var type: ManifoldType? = null + + internal var radiusA: Float = 0.toFloat() + + internal var radiusB: Float = 0.toFloat() + + internal var pointCount: Int = 0 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactRegister.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactRegister.kt new file mode 100644 index 0000000..7a4dcc8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactRegister.kt @@ -0,0 +1,33 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.pooling.IDynamicStack + +class ContactRegister { + + var creator: IDynamicStack? = null + + var primary: Boolean = false +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactSolver.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactSolver.kt new file mode 100644 index 0000000..f1abe9d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactSolver.kt @@ -0,0 +1,1020 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.WorldManifold +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.TimeStep +import org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint +import org.jbox2d.internal.arraycopy + + +/** + * @author Daniel + */ +class ContactSolver { + + var m_step: TimeStep? = null + var m_positions: Array? = null + var m_velocities: Array? = null + var m_positionConstraints: Array = Array(INITIAL_NUM_CONSTRAINTS) { ContactPositionConstraint() } + var m_velocityConstraints: Array = Array(INITIAL_NUM_CONSTRAINTS) { ContactVelocityConstraint() } + var m_contacts: Array? = null + var m_count: Int = 0 + + // djm pooling, and from above + private val xfA = Transform() + private val xfB = Transform() + private val worldManifold = WorldManifold() + + /* + * #if 0 // Sequential solver. bool ContactSolver::SolvePositionConstraints(float baumgarte) { + * float minSeparation = 0.0f; + * + * for (int i = 0; i < m_constraintCount; ++i) { ContactConstraint* c = m_constraints + i; Body* + * bodyA = c.bodyA; Body* bodyB = c.bodyB; float invMassA = bodyA.m_mass * bodyA.m_invMass; float + * invIA = bodyA.m_mass * bodyA.m_invI; float invMassB = bodyB.m_mass * bodyB.m_invMass; float + * invIB = bodyB.m_mass * bodyB.m_invI; + * + * Vec2 normal = c.normal; + * + * // Solve normal constraints for (int j = 0; j < c.pointCount; ++j) { ContactConstraintPoint* + * ccp = c.points + j; + * + * Vec2 r1 = Mul(bodyA.GetXForm().R, ccp.localAnchorA - bodyA.GetLocalCenter()); Vec2 r2 = + * Mul(bodyB.GetXForm().R, ccp.localAnchorB - bodyB.GetLocalCenter()); + * + * Vec2 p1 = bodyA.m_sweep.c + r1; Vec2 p2 = bodyB.m_sweep.c + r2; Vec2 dp = p2 - p1; + * + * // Approximate the current separation. float separation = Dot(dp, normal) + ccp.separation; + * + * // Track max constraint error. minSeparation = Min(minSeparation, separation); + * + * // Prevent large corrections and allow slop. float C = Clamp(baumgarte * (separation + + * _linearSlop), -_maxLinearCorrection, 0.0f); + * + * // Compute normal impulse float impulse = -ccp.equalizedMass * C; + * + * Vec2 P = impulse * normal; + * + * bodyA.m_sweep.c -= invMassA * P; bodyA.m_sweep.a -= invIA * Cross(r1, P); + * bodyA.SynchronizeTransform(); + * + * bodyB.m_sweep.c += invMassB * P; bodyB.m_sweep.a += invIB * Cross(r2, P); + * bodyB.SynchronizeTransform(); } } + * + * // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above + * -_linearSlop. return minSeparation >= -1.5f * _linearSlop; } + */ + + private val psolver = PositionSolverManifold() + + fun init(def: ContactSolverDef) { + // System.out.println("Initializing contact solver"); + m_step = def.step + m_count = def.count + + if (m_positionConstraints.size < m_count) { + val old = m_positionConstraints + m_positionConstraints = arrayOfNulls(MathUtils.max(old.size * 2, m_count)) as Array + arraycopy(old, 0, m_positionConstraints, 0, old.size) + for (i in old.size until m_positionConstraints.size) { + m_positionConstraints[i] = ContactPositionConstraint() + } + } + + if (m_velocityConstraints.size < m_count) { + val old = m_velocityConstraints + m_velocityConstraints = arrayOfNulls(MathUtils.max(old.size * 2, m_count)) as Array + arraycopy(old, 0, m_velocityConstraints, 0, old.size) + for (i in old.size until m_velocityConstraints.size) { + m_velocityConstraints[i] = ContactVelocityConstraint() + } + } + + m_positions = def.positions + m_velocities = def.velocities + m_contacts = def.contacts + + for (i in 0 until m_count) { + // System.out.println("contacts: " + m_count); + val contact = m_contacts!![i] + + val fixtureA = contact.m_fixtureA + val fixtureB = contact.m_fixtureB + val shapeA = fixtureA!!.getShape() + val shapeB = fixtureB!!.getShape() + val radiusA = shapeA!!.radius + val radiusB = shapeB!!.radius + val bodyA = fixtureA.getBody() + val bodyB = fixtureB.getBody() + val manifold = contact.getManifold() + + val pointCount = manifold.pointCount + //assert (pointCount > 0); + + val vc = m_velocityConstraints[i] + vc.friction = contact.m_friction + vc.restitution = contact.m_restitution + vc.tangentSpeed = contact.m_tangentSpeed + vc.indexA = bodyA!!.islandIndex + vc.indexB = bodyB!!.islandIndex + vc.invMassA = bodyA.m_invMass + vc.invMassB = bodyB.m_invMass + vc.invIA = bodyA.m_invI + vc.invIB = bodyB.m_invI + vc.contactIndex = i + vc.pointCount = pointCount + vc.K.setZero() + vc.normalMass.setZero() + + val pc = m_positionConstraints[i] + pc.indexA = bodyA.islandIndex + pc.indexB = bodyB.islandIndex + pc.invMassA = bodyA.m_invMass + pc.invMassB = bodyB.m_invMass + pc.localCenterA.set(bodyA.sweep.localCenter) + pc.localCenterB.set(bodyB.sweep.localCenter) + pc.invIA = bodyA.m_invI + pc.invIB = bodyB.m_invI + pc.localNormal.set(manifold.localNormal) + pc.localPoint.set(manifold.localPoint) + pc.pointCount = pointCount + pc.radiusA = radiusA + pc.radiusB = radiusB + pc.type = manifold.type + + // System.out.println("contact point count: " + pointCount); + for (j in 0 until pointCount) { + val cp = manifold.points[j] + val vcp = vc.points[j] + + if (m_step!!.warmStarting) { + // assert(cp.normalImpulse == 0); + // System.out.println("contact normal impulse: " + cp.normalImpulse); + vcp.normalImpulse = m_step!!.dtRatio * cp.normalImpulse + vcp.tangentImpulse = m_step!!.dtRatio * cp.tangentImpulse + } else { + vcp.normalImpulse = 0f + vcp.tangentImpulse = 0f + } + + vcp.rA.setZero() + vcp.rB.setZero() + vcp.normalMass = 0f + vcp.tangentMass = 0f + vcp.velocityBias = 0f + pc.localPoints[j].x = cp.localPoint.x + pc.localPoints[j].y = cp.localPoint.y + } + } + } + + fun warmStart() { + // Warm start. + for (i in 0 until m_count) { + val vc = m_velocityConstraints[i] + + val indexA = vc.indexA + val indexB = vc.indexB + val mA = vc.invMassA + val iA = vc.invIA + val mB = vc.invMassB + val iB = vc.invIB + val pointCount = vc.pointCount + + val vA = m_velocities!![indexA].v + var wA = m_velocities!![indexA].w + val vB = m_velocities!![indexB].v + var wB = m_velocities!![indexB].w + + val normal = vc.normal + val tangentx = 1.0f * normal.y + val tangenty = -1.0f * normal.x + + for (j in 0 until pointCount) { + val vcp = vc.points[j] + val Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse + val Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse + + wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px) + vA.x -= Px * mA + vA.y -= Py * mA + wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px) + vB.x += Px * mB + vB.y += Py * mB + } + m_velocities!![indexA].w = wA + m_velocities!![indexB].w = wB + } + } + + fun initializeVelocityConstraints() { + + // Warm start. + for (i in 0 until m_count) { + val vc = m_velocityConstraints[i] + val pc = m_positionConstraints[i] + + val radiusA = pc.radiusA + val radiusB = pc.radiusB + val manifold = m_contacts!![vc.contactIndex].getManifold() + + val indexA = vc.indexA + val indexB = vc.indexB + + val mA = vc.invMassA + val mB = vc.invMassB + val iA = vc.invIA + val iB = vc.invIB + val localCenterA = pc.localCenterA + val localCenterB = pc.localCenterB + + val cA = m_positions!![indexA].c + val aA = m_positions!![indexA].a + val vA = m_velocities!![indexA].v + val wA = m_velocities!![indexA].w + + val cB = m_positions!![indexB].c + val aB = m_positions!![indexB].a + val vB = m_velocities!![indexB].v + val wB = m_velocities!![indexB].w + + //assert (manifold.pointCount > 0); + + val xfAq = xfA.q + val xfBq = xfB.q + xfAq.setRadians(aA) + xfBq.setRadians(aB) + xfA.p.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y) + xfA.p.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y) + xfB.p.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y) + xfB.p.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y) + + worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB) + + val vcnormal = vc.normal + vcnormal.x = worldManifold.normal.x + vcnormal.y = worldManifold.normal.y + + val pointCount = vc.pointCount + for (j in 0 until pointCount) { + val vcp = vc.points[j] + val wmPj = worldManifold.points[j] + val vcprA = vcp.rA + val vcprB = vcp.rB + vcprA.x = wmPj.x - cA.x + vcprA.y = wmPj.y - cA.y + vcprB.x = wmPj.x - cB.x + vcprB.y = wmPj.y - cB.y + + val rnA = vcprA.x * vcnormal.y - vcprA.y * vcnormal.x + val rnB = vcprB.x * vcnormal.y - vcprB.y * vcnormal.x + + val kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + vcp.normalMass = if (kNormal > 0.0f) 1.0f / kNormal else 0.0f + + val tangentx = 1.0f * vcnormal.y + val tangenty = -1.0f * vcnormal.x + + val rtA = vcprA.x * tangenty - vcprA.y * tangentx + val rtB = vcprB.x * tangenty - vcprB.y * tangentx + + val kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB + + vcp.tangentMass = if (kTangent > 0.0f) 1.0f / kTangent else 0.0f + + // Setup a velocity bias for restitution. + vcp.velocityBias = 0.0f + val tempx = vB.x + -wB * vcprB.y - vA.x - -wA * vcprA.y + val tempy = vB.y + wB * vcprB.x - vA.y - wA * vcprA.x + val vRel = vcnormal.x * tempx + vcnormal.y * tempy + if (vRel < -Settings.velocityThreshold) { + vcp.velocityBias = -vc.restitution * vRel + } + } + + // If we have two points, then prepare the block solver. + if (vc.pointCount == 2) { + val vcp1 = vc.points[0] + val vcp2 = vc.points[1] + val rn1A = vcp1.rA.x * vcnormal.y - vcp1.rA.y * vcnormal.x + val rn1B = vcp1.rB.x * vcnormal.y - vcp1.rB.y * vcnormal.x + val rn2A = vcp2.rA.x * vcnormal.y - vcp2.rA.y * vcnormal.x + val rn2B = vcp2.rB.x * vcnormal.y - vcp2.rB.y * vcnormal.x + + val k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B + val k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B + val k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { + // K is safe to invert. + vc.K.ex.x = k11 + vc.K.ex.y = k12 + vc.K.ey.x = k12 + vc.K.ey.y = k22 + vc.K.invertToOut(vc.normalMass) + } else { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc.pointCount = 1 + } + } + } + } + + fun storeImpulses() { + for (i in 0 until m_count) { + val vc = m_velocityConstraints[i] + val manifold = m_contacts!![vc.contactIndex].getManifold() + + for (j in 0 until vc.pointCount) { + manifold.points[j].normalImpulse = vc.points[j].normalImpulse + manifold.points[j].tangentImpulse = vc.points[j].tangentImpulse + } + } + } + + /** + * Sequential solver. + */ + fun solvePositionConstraints(): Boolean { + var minSeparation = 0.0f + + for (i in 0 until m_count) { + val pc = m_positionConstraints[i] + + val indexA = pc.indexA + val indexB = pc.indexB + + val mA = pc.invMassA + val iA = pc.invIA + val localCenterA = pc.localCenterA + val localCenterAx = localCenterA.x + val localCenterAy = localCenterA.y + val mB = pc.invMassB + val iB = pc.invIB + val localCenterB = pc.localCenterB + val localCenterBx = localCenterB.x + val localCenterBy = localCenterB.y + val pointCount = pc.pointCount + + val cA = m_positions!![indexA].c + var aA = m_positions!![indexA].a + val cB = m_positions!![indexB].c + var aB = m_positions!![indexB].a + + // Solve normal constraints + for (j in 0 until pointCount) { + val xfAq = xfA.q + val xfBq = xfB.q + xfAq.setRadians(aA) + xfBq.setRadians(aB) + xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy + xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy + xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy + xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy + + val psm = psolver + psm.initialize(pc, xfA, xfB, j) + val normal = psm.normal + val point = psm.point + val separation = psm.separation + + val rAx = point.x - cA.x + val rAy = point.y - cA.y + val rBx = point.x - cB.x + val rBy = point.y - cB.y + + // Track max constraint error. + minSeparation = MathUtils.min(minSeparation, separation) + + // Prevent large corrections and allow slop. + val C = MathUtils.clamp(Settings.baumgarte * (separation + Settings.linearSlop), + -Settings.maxLinearCorrection, 0.0f) + + // Compute the effective mass. + val rnA = rAx * normal.y - rAy * normal.x + val rnB = rBx * normal.y - rBy * normal.x + val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + // Compute normal impulse + val impulse = if (K > 0.0f) -C / K else 0.0f + + val Px = normal.x * impulse + val Py = normal.y * impulse + + cA.x -= Px * mA + cA.y -= Py * mA + aA -= iA * (rAx * Py - rAy * Px) + + cB.x += Px * mB + cB.y += Py * mB + aB += iB * (rBx * Py - rBy * Px) + } + + // m_positions[indexA].c.set(cA); + m_positions!![indexA].a = aA + + // m_positions[indexB].c.set(cB); + m_positions!![indexB].a = aB + } + + // We can't expect minSpeparation >= -linearSlop because we don't + // push the separation above -linearSlop. + return minSeparation >= -3.0f * Settings.linearSlop + } + + // Sequential position solver for position constraints. + fun solveTOIPositionConstraints(toiIndexA: Int, toiIndexB: Int): Boolean { + var minSeparation = 0.0f + + for (i in 0 until m_count) { + val pc = m_positionConstraints[i] + + val indexA = pc.indexA + val indexB = pc.indexB + val localCenterA = pc.localCenterA + val localCenterB = pc.localCenterB + val localCenterAx = localCenterA.x + val localCenterAy = localCenterA.y + val localCenterBx = localCenterB.x + val localCenterBy = localCenterB.y + val pointCount = pc.pointCount + + var mA = 0.0f + var iA = 0.0f + if (indexA == toiIndexA || indexA == toiIndexB) { + mA = pc.invMassA + iA = pc.invIA + } + + var mB = 0f + var iB = 0f + if (indexB == toiIndexA || indexB == toiIndexB) { + mB = pc.invMassB + iB = pc.invIB + } + + val cA = m_positions!![indexA].c + var aA = m_positions!![indexA].a + + val cB = m_positions!![indexB].c + var aB = m_positions!![indexB].a + + // Solve normal constraints + for (j in 0 until pointCount) { + val xfAq = xfA.q + val xfBq = xfB.q + xfAq.setRadians(aA) + xfBq.setRadians(aB) + xfA.p.x = cA.x - xfAq.c * localCenterAx + xfAq.s * localCenterAy + xfA.p.y = cA.y - xfAq.s * localCenterAx - xfAq.c * localCenterAy + xfB.p.x = cB.x - xfBq.c * localCenterBx + xfBq.s * localCenterBy + xfB.p.y = cB.y - xfBq.s * localCenterBx - xfBq.c * localCenterBy + + val psm = psolver + psm.initialize(pc, xfA, xfB, j) + val normal = psm.normal + + val point = psm.point + val separation = psm.separation + + val rAx = point.x - cA.x + val rAy = point.y - cA.y + val rBx = point.x - cB.x + val rBy = point.y - cB.y + + // Track max constraint error. + minSeparation = MathUtils.min(minSeparation, separation) + + // Prevent large corrections and allow slop. + val C = MathUtils.clamp(Settings.toiBaugarte * (separation + Settings.linearSlop), + -Settings.maxLinearCorrection, 0.0f) + + // Compute the effective mass. + val rnA = rAx * normal.y - rAy * normal.x + val rnB = rBx * normal.y - rBy * normal.x + val K = mA + mB + iA * rnA * rnA + iB * rnB * rnB + + // Compute normal impulse + val impulse = if (K > 0.0f) -C / K else 0.0f + + val Px = normal.x * impulse + val Py = normal.y * impulse + + cA.x -= Px * mA + cA.y -= Py * mA + aA -= iA * (rAx * Py - rAy * Px) + + cB.x += Px * mB + cB.y += Py * mB + aB += iB * (rBx * Py - rBy * Px) + } + + // m_positions[indexA].c.set(cA); + m_positions!![indexA].a = aA + + // m_positions[indexB].c.set(cB); + m_positions!![indexB].a = aB + } + + // We can't expect minSpeparation >= -_linearSlop because we don't + // push the separation above -_linearSlop. + return minSeparation >= -1.5f * Settings.linearSlop + } + + + private var vA: Vec2? = null + private var vB: Vec2? = null + private var wA: Float = 0.toFloat() + private var wB: Float = 0.toFloat() + + fun solveVelocityConstraints() { + for (i in 0 until m_count) { + val vc = m_velocityConstraints[i] + + val indexA = vc.indexA + val indexB = vc.indexB + + val mA = vc.invMassA + val mB = vc.invMassB + val iA = vc.invIA + val iB = vc.invIB + val pointCount = vc.pointCount + + vA = m_velocities!![indexA].v + wA = m_velocities!![indexA].w + vB = m_velocities!![indexB].v + wB = m_velocities!![indexB].w + + val normal = vc.normal + val normalx = normal.x + val normaly = normal.y + val tangentx = 1.0f * vc.normal.y + val tangenty = -1.0f * vc.normal.x + val friction = vc.friction + + //assert (pointCount == 1 || pointCount == 2); + solveVelocityConstraints0(vc, mA, mB, iA, iB, pointCount, tangentx, tangenty, friction) + + // Solve normal constraints + if (vc.pointCount == 1) { + solveVelocityConstraints1(vc.points[0], mA, mB, iA, iB, normalx, normaly) + } else { + solveVelocityConstraints2(vc, mA, mB, iA, iB, normal, normalx, normaly) + } + + // m_velocities[indexA].v.set(vA); + m_velocities!![indexA].w = wA + // m_velocities[indexB].v.set(vB); + m_velocities!![indexB].w = wB + } + } + + private fun solveVelocityConstraints2(vc: ContactVelocityConstraint, mA: Float, mB: Float, iA: Float, iB: Float, normal: Vec2, normalx: Float, normaly: Float) { + // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on + // Box2D_Lite). + // Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn_0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). The complementary + // constraint vn_i * x_i + // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D + // contact problem the cases + // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be + // tested. The first valid + // solution that satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the iterative nature of + // the solver which only requires + // that the accumulated impulse is clamped and not the incremental impulse) we change the + // impulse variable (x_i). + // + // Substitute: + // + // x = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental impulse + // to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + val cp1 = vc.points[0] + val cp2 = vc.points[1] + val cp1rA = cp1.rA + val cp1rB = cp1.rB + val cp2rA = cp2.rA + val cp2rB = cp2.rB + val ax = cp1.normalImpulse + val ay = cp2.normalImpulse + + //assert (ax >= 0.0f && ay >= 0.0f); + // Relative velocity at contact + // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA); + val dv1x = -wB * cp1rB.y + vB!!.x - vA!!.x + wA * cp1rA.y + val dv1y = wB * cp1rB.x + vB!!.y - vA!!.y - wA * cp1rA.x + + // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); + val dv2x = -wB * cp2rB.y + vB!!.x - vA!!.x + wA * cp2rA.y + val dv2y = wB * cp2rB.x + vB!!.y - vA!!.y - wA * cp2rA.x + + // Compute normal velocity + var vn1 = dv1x * normalx + dv1y * normaly + var vn2 = dv2x * normalx + dv2y * normaly + + var bx = vn1 - cp1.velocityBias + var by = vn2 - cp2.velocityBias + + // Compute b' + val R = vc.K + bx -= R.ex.x * ax + R.ey.x * ay + by -= R.ex.y * ax + R.ey.y * ay + + // final float k_errorTol = 1e-3f; + // B2_NOT_USED(k_errorTol); + // + // Case 1: vn = 0 + // + // 0 = A * x' + b' + // + // Solve for x': + // + // x' = - inv(A) * b' + // + // Vec2 x = - Mul(c.normalMass, b); + val R1 = vc.normalMass + var xx = R1.ex.x * bx + R1.ey.x * by + var xy = R1.ex.y * bx + R1.ey.y * by + xx *= -1f + xy *= -1f + + if (xx >= 0.0f && xy >= 0.0f) { + solveVelocityConstraints2a(mA, mB, iA, iB, normal, normalx, normaly, cp1, cp2, cp1rA, cp1rB, cp2rA, cp2rB, ax, ay, xx, xy) + } else { + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1' + a12 * 0 + b1' + // vn2 = a21 * x1' + a22 * 0 + ' + // + xx = -cp1.normalMass * bx + xy = 0.0f + vn1 = 0.0f + vn2 = vc.K.ex.y * xx + by + + if (xx >= 0.0f && vn2 >= 0.0f) { + solveVelocityConstraints2b(mA, mB, iA, iB, normal, normalx, normaly, cp1, cp2, cp1rA, cp1rB, cp2rA, cp2rB, ax, ay, xx, xy) + } else { + + // + // Case 3: wB = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2' + b1' + // 0 = a21 * 0 + a22 * x2' + ' + // + xx = 0.0f + xy = -cp2.normalMass * by + vn1 = vc.K.ey.x * xy + bx + vn2 = 0.0f + + if (xy >= 0.0f && vn1 >= 0.0f) { + solveVelocityConstraints2c(mA, mB, iA, iB, normal, normalx, normaly, cp1, cp2, cp1rA, cp1rB, cp2rA, cp2rB, ax, ay, xx, xy) + } else { + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = ; + xx = 0.0f + xy = 0.0f + vn1 = bx + vn2 = by + + if (vn1 >= 0.0f && vn2 >= 0.0f) { + solveVelocityConstraints2d(mA, mB, iA, iB, normalx, normaly, cp1, cp2, cp1rA, cp1rB, cp2rA, cp2rB, ax, ay, xx, xy) + } else { + // No solution, give up. This is hit sometimes, but it doesn't seem to matter. + } + } + } + } + } + + private fun solveVelocityConstraints2d(mA: Float, mB: Float, iA: Float, iB: Float, normalx: Float, normaly: Float, cp1: VelocityConstraintPoint, cp2: VelocityConstraintPoint, cp1rA: Vec2, cp1rB: Vec2, cp2rA: Vec2, cp2rB: Vec2, ax: Float, ay: Float, xx: Float, xy: Float) { + // Resubstitute for the incremental impulse + val dx = xx - ax + val dy = xy - ay + + // Apply incremental impulse + /* + * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= + * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); + * + * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); + */ + + val P1x = normalx * dx + val P1y = normaly * dx + val P2x = normalx * dy + val P2y = normaly * dy + + vA!!.x -= mA * (P1x + P2x) + vA!!.y -= mA * (P1y + P2y) + vB!!.x += mB * (P1x + P2x) + vB!!.y += mB * (P1y + P2y) + + wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x)) + wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x)) + + // Accumulate + cp1.normalImpulse = xx + cp2.normalImpulse = xy + } + + private fun solveVelocityConstraints2c(mA: Float, mB: Float, iA: Float, iB: Float, normal: Vec2, normalx: Float, normaly: Float, cp1: VelocityConstraintPoint, cp2: VelocityConstraintPoint, cp1rA: Vec2, cp1rB: Vec2, cp2rA: Vec2, cp2rB: Vec2, ax: Float, ay: Float, xx: Float, xy: Float) { + val vn2: Float// Resubstitute for the incremental impulse + val dx = xx - ax + val dy = xy - ay + + // Apply incremental impulse + /* + * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= + * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); + * + * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); + */ + + val P1x = normalx * dx + val P1y = normaly * dx + val P2x = normalx * dy + val P2y = normaly * dy + + vA!!.x -= mA * (P1x + P2x) + vA!!.y -= mA * (P1y + P2y) + vB!!.x += mB * (P1x + P2x) + vB!!.y += mB * (P1y + P2y) + + wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x)) + wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x)) + + // Accumulate + cp1.normalImpulse = xx + cp2.normalImpulse = xy + + /* + * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA - + * Cross(wA, cp2.rA); + * + * // Compute normal velocity vn2 = Dot(dv2, normal); + * + * assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif + */ + if (DEBUG_SOLVER) { + // Postconditions + val dv2 = vB!!.add(Vec2.cross(wB, cp2rB).subLocal(vA!!).subLocal(Vec2.cross(wA, cp2rA))) + // Compute normal velocity + vn2 = Vec2.dot(dv2, normal) + + //assert (MathUtils.abs(vn2 - cp2.velocityBias) < k_errorTol); + } + } + + private fun solveVelocityConstraints2b(mA: Float, mB: Float, iA: Float, iB: Float, normal: Vec2, normalx: Float, normaly: Float, cp1: VelocityConstraintPoint, cp2: VelocityConstraintPoint, cp1rA: Vec2, cp1rB: Vec2, cp2rA: Vec2, cp2rB: Vec2, ax: Float, ay: Float, xx: Float, xy: Float) { + val vn1: Float// Get the incremental impulse + val dx = xx - ax + val dy = xy - ay + + // Apply incremental impulse + // Vec2 P1 = d.x * normal; + // Vec2 P2 = d.y * normal; + val P1x = normalx * dx + val P1y = normaly * dx + val P2x = normalx * dy + val P2y = normaly * dy + + /* + * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= + * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); + * + * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); + */ + + vA!!.x -= mA * (P1x + P2x) + vA!!.y -= mA * (P1y + P2y) + vB!!.x += mB * (P1x + P2x) + vB!!.y += mB * (P1y + P2y) + + wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x)) + wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x)) + + // Accumulate + cp1.normalImpulse = xx + cp2.normalImpulse = xy + + /* + * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - + * Cross(wA, cp1.rA); + * + * // Compute normal velocity vn1 = Dot(dv1, normal); + * + * assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif + */ + if (DEBUG_SOLVER) { + // Postconditions + val dv1 = vB!!.add(Vec2.cross(wB, cp1rB).subLocal(vA!!).subLocal(Vec2.cross(wA, cp1rA))) + // Compute normal velocity + vn1 = Vec2.dot(dv1, normal) + + //assert (MathUtils.abs(vn1 - cp1.velocityBias) < k_errorTol); + } + } + + private fun solveVelocityConstraints2a(mA: Float, mB: Float, iA: Float, iB: Float, normal: Vec2, normalx: Float, normaly: Float, cp1: VelocityConstraintPoint, cp2: VelocityConstraintPoint, cp1rA: Vec2, cp1rB: Vec2, cp2rA: Vec2, cp2rB: Vec2, ax: Float, ay: Float, xx: Float, xy: Float) { + val vn1: Float + val vn2: Float// Get the incremental impulse + // Vec2 d = x - a; + val dx = xx - ax + val dy = xy - ay + + // Apply incremental impulse + // Vec2 P1 = d.x * normal; + // Vec2 P2 = d.y * normal; + val P1x = dx * normalx + val P1y = dx * normaly + val P2x = dy * normalx + val P2y = dy * normaly + + /* + * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2)); + * + * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2)); + */ + + vA!!.x -= mA * (P1x + P2x) + vA!!.y -= mA * (P1y + P2y) + vB!!.x += mB * (P1x + P2x) + vB!!.y += mB * (P1y + P2y) + + wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x)) + wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x)) + + // Accumulate + cp1.normalImpulse = xx + cp2.normalImpulse = xy + + /* + * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA - + * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA); + * + * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal); + * + * assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); assert(Abs(vn2 - cp2.velocityBias) + * < k_errorTol); #endif + */ + if (DEBUG_SOLVER) { + // Postconditions + val dv1 = vB!!.add(Vec2.cross(wB, cp1rB).subLocal(vA!!).subLocal(Vec2.cross(wA, cp1rA))) + val dv2 = vB!!.add(Vec2.cross(wB, cp2rB).subLocal(vA!!).subLocal(Vec2.cross(wA, cp2rA))) + // Compute normal velocity + vn1 = Vec2.dot(dv1, normal) + vn2 = Vec2.dot(dv2, normal) + + //assert (MathUtils.abs(vn1 - cp1.velocityBias) < k_errorTol); + //assert (MathUtils.abs(vn2 - cp2.velocityBias) < k_errorTol); + } + } + + private fun solveVelocityConstraints1(point: VelocityConstraintPoint, mA: Float, mB: Float, iA: Float, iB: Float, normalx: Float, normaly: Float) { + + // Relative velocity at contact + // Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA); + + val dvx = -wB * point.rB.y + vB!!.x - vA!!.x + wA * point.rA.y + val dvy = wB * point.rB.x + vB!!.y - vA!!.y - wA * point.rA.x + + // Compute normal impulse + val vn = dvx * normalx + dvy * normaly + var lambda = -point.normalMass * (vn - point.velocityBias) + + // Clamp the accumulated impulse + val a = point.normalImpulse + lambda + val newImpulse = if (a > 0.0f) a else 0.0f + lambda = newImpulse - point.normalImpulse + point.normalImpulse = newImpulse + + // Apply contact impulse + val Px = normalx * lambda + val Py = normaly * lambda + + // vA -= invMassA * P; + vA!!.x -= Px * mA + vA!!.y -= Py * mA + wA -= iA * (point.rA.x * Py - point.rA.y * Px) + + // vB += invMassB * P; + vB!!.x += Px * mB + vB!!.y += Py * mB + wB += iB * (point.rB.x * Py - point.rB.y * Px) + } + + private fun solveVelocityConstraints0(vc: ContactVelocityConstraint, mA: Float, mB: Float, iA: Float, iB: Float, pointCount: Int, tangentx: Float, tangenty: Float, friction: Float) { + // Solve tangent constraints + for (j in 0 until pointCount) { + val vcp = vc.points[j] + val a = vcp.rA + val dvx = -wB * vcp.rB.y + vB!!.x - vA!!.x + wA * a.y + val dvy = wB * vcp.rB.x + vB!!.y - vA!!.y - wA * a.x + + // Compute tangent force + val vt = dvx * tangentx + dvy * tangenty - vc.tangentSpeed + var lambda = vcp.tangentMass * -vt + + // Clamp the accumulated force + val maxFriction = friction * vcp.normalImpulse + val newImpulse = MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction) + lambda = newImpulse - vcp.tangentImpulse + vcp.tangentImpulse = newImpulse + + // Apply contact impulse + // Vec2 P = lambda * tangent; + + val Px = tangentx * lambda + val Py = tangenty * lambda + + // vA -= invMassA * P; + vA!!.x -= Px * mA + vA!!.y -= Py * mA + wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px) + + // vB += invMassB * P; + vB!!.x += Px * mB + vB!!.y += Py * mB + wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px) + } + } + + + class ContactSolverDef { + var step: TimeStep? = null + var contacts: Array? = null + var count: Int = 0 + var positions: Array? = null + var velocities: Array? = null + } + + companion object { + + val DEBUG_SOLVER = false + val k_errorTol = 1e-3f + /** + * For each solver, this is the initial number of constraints in the array, which expands as + * needed. + */ + val INITIAL_NUM_CONSTRAINTS = 256 + + /** + * Ensure a reasonable condition number. for the block solver + */ + val k_maxConditionNumber = 100.0f + } +} + + diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactVelocityConstraint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactVelocityConstraint.kt new file mode 100644 index 0000000..b2fdcd8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/ContactVelocityConstraint.kt @@ -0,0 +1,78 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.common.Mat22 +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 + +class ContactVelocityConstraint { + + var points = Array(Settings.maxManifoldPoints) { VelocityConstraintPoint() } + + val normal = Vec2() + + val normalMass = Mat22() + + val K = Mat22() + + var indexA: Int = 0 + + var indexB: Int = 0 + + var invMassA: Float = 0.toFloat() + + var invMassB: Float = 0.toFloat() + + var invIA: Float = 0.toFloat() + + var invIB: Float = 0.toFloat() + + var friction: Float = 0.toFloat() + + var restitution: Float = 0.toFloat() + + var tangentSpeed: Float = 0.toFloat() + + var pointCount: Int = 0 + + var contactIndex: Int = 0 + + class VelocityConstraintPoint { + + val rA = Vec2() + + val rB = Vec2() + + var normalImpulse: Float = 0.toFloat() + + var tangentImpulse: Float = 0.toFloat() + + var normalMass: Float = 0.toFloat() + + var tangentMass: Float = 0.toFloat() + + var velocityBias: Float = 0.toFloat() + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndCircleContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndCircleContact.kt new file mode 100644 index 0000000..3dac5c9 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndCircleContact.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.CircleShape +import org.jbox2d.collision.shapes.EdgeShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class EdgeAndCircleContact(argPool: IWorldPool) : Contact(argPool) { + + override fun init(fA: Fixture, indexA: Int, fB: Fixture, indexB: Int) { + super.init(fA, indexA, fB, indexB) + assert(m_fixtureA!!.type === ShapeType.EDGE) + assert(m_fixtureB!!.type === ShapeType.CIRCLE) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + pool.collision.collideEdgeAndCircle(manifold, m_fixtureA!!.m_shape as EdgeShape, xfA, + m_fixtureB!!.m_shape as CircleShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndPolygonContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndPolygonContact.kt new file mode 100644 index 0000000..81dbf51 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/EdgeAndPolygonContact.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.EdgeShape +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class EdgeAndPolygonContact(argPool: IWorldPool) : Contact(argPool) { + + override fun init(fA: Fixture, indexA: Int, fB: Fixture, indexB: Int) { + super.init(fA, indexA, fB, indexB) + assert(m_fixtureA!!.type === ShapeType.EDGE) + assert(m_fixtureB!!.type === ShapeType.POLYGON) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + pool.collision.collideEdgeAndPolygon(manifold, m_fixtureA!!.m_shape as EdgeShape, xfA, + m_fixtureB!!.m_shape as PolygonShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonAndCircleContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonAndCircleContact.kt new file mode 100644 index 0000000..411d1be --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonAndCircleContact.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.CircleShape +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class PolygonAndCircleContact(argPool: IWorldPool) : Contact(argPool) { + + fun init(fixtureA: Fixture, fixtureB: Fixture) { + super.init(fixtureA, 0, fixtureB, 0) + assert(m_fixtureA!!.type === ShapeType.POLYGON) + assert(m_fixtureB!!.type === ShapeType.CIRCLE) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + pool.collision.collidePolygonAndCircle(manifold, m_fixtureA!!.m_shape as PolygonShape, + xfA, m_fixtureB!!.m_shape as CircleShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonContact.kt new file mode 100644 index 0000000..233b583 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PolygonContact.kt @@ -0,0 +1,46 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.collision.shapes.ShapeType +import org.jbox2d.common.Transform +import org.jbox2d.dynamics.Fixture +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +class PolygonContact(argPool: IWorldPool) : Contact(argPool) { + + fun init(fixtureA: Fixture, fixtureB: Fixture) { + super.init(fixtureA, 0, fixtureB, 0) + assert(m_fixtureA!!.type == ShapeType.POLYGON) + assert(m_fixtureB!!.type == ShapeType.POLYGON) + } + + override fun evaluate(manifold: Manifold, xfA: Transform, xfB: Transform) { + pool.collision.collidePolygons(manifold, m_fixtureA!!.m_shape as PolygonShape, xfA, + m_fixtureB!!.m_shape as PolygonShape, xfB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Position.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Position.kt new file mode 100644 index 0000000..4058c4e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Position.kt @@ -0,0 +1,33 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.common.Vec2 + +class Position { + + val c = Vec2() + + var a: Float = 0.toFloat() +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PositionSolverManifold.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PositionSolverManifold.kt new file mode 100644 index 0000000..c7a7638 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/PositionSolverManifold.kt @@ -0,0 +1,106 @@ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.collision.Manifold +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert + +internal class PositionSolverManifold { + + val normal = Vec2() + + val point = Vec2() + + var separation: Float = 0.toFloat() + + fun initialize(pc: ContactPositionConstraint, xfA: Transform, xfB: Transform, index: Int) { + assert(pc.pointCount > 0) + + val xfAq = xfA.q + val xfBq = xfB.q + val pcLocalPointsI = pc.localPoints[index] + when (pc.type) { + Manifold.ManifoldType.CIRCLES -> { + // Transform.mulToOutUnsafe(xfA, pc.localPoint, pointA); + // Transform.mulToOutUnsafe(xfB, pc.localPoints[0], pointB); + // normal.set(pointB).subLocal(pointA); + // normal.normalize(); + // + // point.set(pointA).addLocal(pointB).mulLocal(.5f); + // temp.set(pointB).subLocal(pointA); + // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; + val plocalPoint = pc.localPoint + val pLocalPoints0 = pc.localPoints[0] + val pointAx = xfAq.c * plocalPoint.x - xfAq.s * plocalPoint.y + xfA.p.x + val pointAy = xfAq.s * plocalPoint.x + xfAq.c * plocalPoint.y + xfA.p.y + val pointBx = xfBq.c * pLocalPoints0.x - xfBq.s * pLocalPoints0.y + xfB.p.x + val pointBy = xfBq.s * pLocalPoints0.x + xfBq.c * pLocalPoints0.y + xfB.p.y + normal.x = pointBx - pointAx + normal.y = pointBy - pointAy + normal.normalize() + + point.x = (pointAx + pointBx) * .5f + point.y = (pointAy + pointBy) * .5f + val tempx = pointBx - pointAx + val tempy = pointBy - pointAy + separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB + } + + Manifold.ManifoldType.FACE_A -> { + // Rot.mulToOutUnsafe(xfAq, pc.localNormal, normal); + // Transform.mulToOutUnsafe(xfA, pc.localPoint, planePoint); + // + // Transform.mulToOutUnsafe(xfB, pc.localPoints[index], clipPoint); + // temp.set(clipPoint).subLocal(planePoint); + // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; + // point.set(clipPoint); + val pcLocalNormal = pc.localNormal + val pcLocalPoint = pc.localPoint + normal.x = xfAq.c * pcLocalNormal.x - xfAq.s * pcLocalNormal.y + normal.y = xfAq.s * pcLocalNormal.x + xfAq.c * pcLocalNormal.y + val planePointx = xfAq.c * pcLocalPoint.x - xfAq.s * pcLocalPoint.y + xfA.p.x + val planePointy = xfAq.s * pcLocalPoint.x + xfAq.c * pcLocalPoint.y + xfA.p.y + + val clipPointx = xfBq.c * pcLocalPointsI.x - xfBq.s * pcLocalPointsI.y + xfB.p.x + val clipPointy = xfBq.s * pcLocalPointsI.x + xfBq.c * pcLocalPointsI.y + xfB.p.y + val tempx = clipPointx - planePointx + val tempy = clipPointy - planePointy + separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB + point.x = clipPointx + point.y = clipPointy + } + + Manifold.ManifoldType.FACE_B -> { + // Rot.mulToOutUnsafe(xfBq, pc.localNormal, normal); + // Transform.mulToOutUnsafe(xfB, pc.localPoint, planePoint); + // + // Transform.mulToOutUnsafe(xfA, pcLocalPointsI, clipPoint); + // temp.set(clipPoint).subLocal(planePoint); + // separation = Vec2.dot(temp, normal) - pc.radiusA - pc.radiusB; + // point.set(clipPoint); + // + // // Ensure normal points from A to B + // normal.negateLocal(); + val pcLocalNormal = pc.localNormal + val pcLocalPoint = pc.localPoint + normal.x = xfBq.c * pcLocalNormal.x - xfBq.s * pcLocalNormal.y + normal.y = xfBq.s * pcLocalNormal.x + xfBq.c * pcLocalNormal.y + val planePointx = xfBq.c * pcLocalPoint.x - xfBq.s * pcLocalPoint.y + xfB.p.x + val planePointy = xfBq.s * pcLocalPoint.x + xfBq.c * pcLocalPoint.y + xfB.p.y + + val clipPointx = xfAq.c * pcLocalPointsI.x - xfAq.s * pcLocalPointsI.y + xfA.p.x + val clipPointy = xfAq.s * pcLocalPointsI.x + xfAq.c * pcLocalPointsI.y + xfA.p.y + val tempx = clipPointx - planePointx + val tempy = clipPointy - planePointy + separation = tempx * normal.x + tempy * normal.y - pc.radiusA - pc.radiusB + point.x = clipPointx + point.y = clipPointy + normal.x *= -1f + normal.y *= -1f + } + + else -> { + } + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Velocity.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Velocity.kt new file mode 100644 index 0000000..bb65655 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/contacts/Velocity.kt @@ -0,0 +1,33 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.contacts + +import org.jbox2d.common.Vec2 + +class Velocity { + + val v = Vec2() + + var w: Float = 0.toFloat() +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJoint.kt new file mode 100644 index 0000000..384cc3e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJoint.kt @@ -0,0 +1,233 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body +import org.jbox2d.dynamics.SolverData +import org.jbox2d.dynamics.World +import org.jbox2d.dynamics.contacts.Position + +class ConstantVolumeJoint( + private val world: World, + def: ConstantVolumeJointDef +) : Joint(world.pool, def) { + + val bodies: Array + private val targetLengths: FloatArray + private var targetVolume: Float = 0.toFloat() + + private val normals: Array + private var m_impulse = 0.0f + + var joints: Array; private set + + private val bodyArea: Float + get() { + var area = 0.0f + for (i in bodies.indices) { + val next = if (i == bodies.size - 1) 0 else i + 1 + area += bodies[i].worldCenter.x * bodies[next].worldCenter.y - bodies[next].worldCenter.x * bodies[i].worldCenter.y + } + area *= .5f + return area + } + + fun inflate(factor: Float) { + targetVolume *= factor + } + + init { + if (def.bodies.size <= 2) { + throw IllegalArgumentException( + "You cannot create a constant volume joint with less than three bodies.") + } + bodies = def.bodies.toTypedArray() + + targetLengths = FloatArray(bodies.size) + for (i in targetLengths.indices) { + val next = if (i == targetLengths.size - 1) 0 else i + 1 + val dist = bodies[i].worldCenter.sub(bodies[next].worldCenter).length() + targetLengths[i] = dist + } + targetVolume = bodyArea + + if (def.joints != null && def.joints!!.size != def.bodies.size) { + throw IllegalArgumentException( + "Incorrect joint definition. Joints have to correspond to the bodies") + } + if (def.joints == null) { + val djd = DistanceJointDef() + joints = arrayOfNulls(bodies.size) as Array + for (i in targetLengths.indices) { + val next = if (i == targetLengths.size - 1) 0 else i + 1 + djd.frequencyHz = def.frequencyHz// 20.0f; + djd.dampingRatio = def.dampingRatio// 50.0f; + djd.collideConnected = def.collideConnected + djd.initialize(bodies[i], bodies[next], bodies[i].worldCenter, + bodies[next].worldCenter) + joints[i] = world.createJoint(djd) as DistanceJoint + } + } else { + joints = def.joints!!.toTypedArray() + } + + normals = Array(bodies.size) { Vec2() } + } + + override fun destructor() { + for (i in joints!!.indices) { + world.destroyJoint(joints!![i]) + } + } + + private fun getSolverArea(positions: Array): Float { + var area = 0.0f + for (i in bodies.indices) { + val next = if (i == bodies.size - 1) 0 else i + 1 + area += positions[bodies[i].islandIndex].c.x * positions[bodies[next].islandIndex].c.y - positions[bodies[next].islandIndex].c.x * positions[bodies[i].islandIndex].c.y + } + area *= .5f + return area + } + + private fun constrainEdges(positions: Array): Boolean { + var perimeter = 0.0f + for (i in bodies.indices) { + val next = if (i == bodies.size - 1) 0 else i + 1 + val dx = positions!![bodies[next].islandIndex].c.x - positions[bodies[i].islandIndex].c.x + val dy = positions[bodies[next].islandIndex].c.y - positions[bodies[i].islandIndex].c.y + var dist = MathUtils.sqrt(dx * dx + dy * dy) + if (dist < Settings.EPSILON) { + dist = 1.0f + } + normals[i].x = dy / dist + normals[i].y = -dx / dist + perimeter += dist + } + + val delta = pool.popVec2() + + val deltaArea = targetVolume - getSolverArea(positions) + val toExtrude = 0.5f * deltaArea / perimeter // *relaxationFactor + // float sumdeltax = 0.0f; + var done = true + for (i in bodies.indices) { + val next = if (i == bodies.size - 1) 0 else i + 1 + delta.set(toExtrude * (normals[i].x + normals[next].x), toExtrude * (normals[i].y + normals[next].y)) + // sumdeltax += dx; + val normSqrd = delta.lengthSquared() + if (normSqrd > Settings.maxLinearCorrection * Settings.maxLinearCorrection) { + delta.mulLocal(Settings.maxLinearCorrection / MathUtils.sqrt(normSqrd)) + } + if (normSqrd > Settings.linearSlop * Settings.linearSlop) { + done = false + } + positions!![bodies[next].islandIndex].c.x += delta.x + positions!![bodies[next].islandIndex].c.y += delta.y + // bodies[next].m_linearVelocity.x += delta.x * step.inv_dt; + // bodies[next].m_linearVelocity.y += delta.y * step.inv_dt; + } + + pool.pushVec2(1) + // System.out.println(sumdeltax); + return done + } + + override fun initVelocityConstraints(step: SolverData) { + val velocities = step.velocities + val positions = step.positions + val d = pool.getVec2Array(bodies.size) + + for (i in bodies.indices) { + val prev = if (i == 0) bodies.size - 1 else i - 1 + val next = if (i == bodies.size - 1) 0 else i + 1 + d[i].set(positions!![bodies[next].islandIndex].c) + d[i].subLocal(positions[bodies[prev].islandIndex].c) + } + + if (step.step!!.warmStarting) { + m_impulse *= step.step!!.dtRatio + // float lambda = -2.0f * crossMassSum / dotMassSum; + // System.out.println(crossMassSum + " " +dotMassSum); + // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, + // Settings.maxLinearCorrection); + // m_impulse = lambda; + for (i in bodies.indices) { + velocities!![bodies[i].islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * m_impulse + velocities!![bodies[i].islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * m_impulse + } + } else { + m_impulse = 0.0f + } + } + + override fun solvePositionConstraints(step: SolverData): Boolean { + return constrainEdges(step.positions!!) + } + + override fun solveVelocityConstraints(step: SolverData) { + var crossMassSum = 0.0f + var dotMassSum = 0.0f + + val velocities = step.velocities + val positions = step.positions + val d = pool.getVec2Array(bodies.size) + + for (i in bodies.indices) { + val prev = if (i == 0) bodies.size - 1 else i - 1 + val next = if (i == bodies.size - 1) 0 else i + 1 + d[i].set(positions!![bodies[next].islandIndex].c) + d[i].subLocal(positions[bodies[prev].islandIndex].c) + dotMassSum += d[i].lengthSquared() / bodies[i].m_mass + crossMassSum += Vec2.cross(velocities!![bodies[i].islandIndex].v, d[i]) + } + val lambda = -2.0f * crossMassSum / dotMassSum + // System.out.println(crossMassSum + " " +dotMassSum); + // lambda = MathUtils.clamp(lambda, -Settings.maxLinearCorrection, + // Settings.maxLinearCorrection); + m_impulse += lambda + // System.out.println(m_impulse); + for (i in bodies.indices) { + velocities!![bodies[i].islandIndex].v.x += bodies[i].m_invMass * d[i].y * .5f * lambda + velocities!![bodies[i].islandIndex].v.y += bodies[i].m_invMass * -d[i].x * .5f * lambda + } + } + + /** No-op */ + override fun getAnchorA(argOut: Vec2) {} + + /** No-op */ + override fun getAnchorB(argOut: Vec2) {} + + /** No-op */ + override fun getReactionForce(inv_dt: Float, argOut: Vec2) {} + + /** No-op */ + override fun getReactionTorque(inv_dt: Float): Float { + return 0f + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJointDef.kt new file mode 100644 index 0000000..65264c2 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/ConstantVolumeJointDef.kt @@ -0,0 +1,72 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.dynamics.Body + +/** + * Definition for a [ConstantVolumeJoint], which connects a group a bodies together so they + * maintain a constant volume within them. + */ +class ConstantVolumeJointDef : JointDef(JointType.CONSTANT_VOLUME) { + + var frequencyHz: Float = 0f + + var dampingRatio: Float = 0f + + internal var bodies: ArrayList = ArrayList() + internal var joints: ArrayList? = null + + init { + collideConnected = false + frequencyHz = 0.0f + dampingRatio = 0.0f + } + + /** + * Adds a body to the group + * + * @param argBody + */ + fun addBody(argBody: Body) { + bodies.add(argBody) + if (bodies.size == 1) { + bodyA = argBody + } + if (bodies.size == 2) { + bodyB = argBody + } + } + + /** + * Adds a body and the pre-made distance joint. Should only be used for deserialization. + */ + fun addBodyAndJoint(argBody: Body, argJoint: DistanceJoint) { + addBody(argBody) + if (joints == null) { + joints = ArrayList() + } + joints!!.add(argJoint) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJoint.kt new file mode 100644 index 0000000..33e9747 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJoint.kt @@ -0,0 +1,316 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.pooling.IWorldPool + +//C = norm(p2 - p1) - L +//u = (p2 - p1) / norm(p2 - p1) +//Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +//J = [-u -cross(r1, u) u cross(r2, u)] +//K = J * invM * JT +//= invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +/** + * A distance joint constrains two points on two bodies to remain at a fixed distance from each + * other. You can view this as a massless, rigid rod. + */ +class DistanceJoint(argWorld: IWorldPool, def: DistanceJointDef) : Joint(argWorld, def) { + + var frequency: Float = 0.toFloat() + var dampingRatio: Float = 0.toFloat() + private var m_bias: Float = 0.toFloat() + + // Solver shared + val localAnchorA: Vec2 + val localAnchorB: Vec2 + private var m_gamma: Float = 0.toFloat() + private var m_impulse: Float = 0.toFloat() + var length: Float = 0.toFloat() + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_u = Vec2() + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private var m_mass: Float = 0.toFloat() + + init { + localAnchorA = def.localAnchorA.clone() + localAnchorB = def.localAnchorB.clone() + length = def.length + m_impulse = 0.0f + frequency = def.frequencyHz + dampingRatio = def.dampingRatio + m_gamma = 0.0f + m_bias = 0.0f + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + /** + * Get the reaction force given the inverse time step. Unit is N. + */ + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.x = m_impulse * m_u.x * inv_dt + argOut.y = m_impulse * m_u.y * inv_dt + } + + /** + * Get the reaction torque given the inverse time step. Unit is N*m. This is always zero for a + * distance joint. + */ + override fun getReactionTorque(inv_dt: Float): Float { + return 0.0f + } + + override fun initVelocityConstraints(data: SolverData) { + + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + + qA.setRadians(aA) + qB.setRadians(aB) + + // use m_u as temporary variable + Rot.mulToOutUnsafe(qA, m_u.set(localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, m_u.set(localAnchorB).subLocal(m_localCenterB), m_rB) + m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA) + + pool.pushRot(2) + + // Handle singularity. + val length = m_u.length() + if (length > Settings.linearSlop) { + m_u.x *= 1.0f / length + m_u.y *= 1.0f / length + } else { + m_u.set(0.0f, 0.0f) + } + + + val crAu = Vec2.cross(m_rA, m_u) + val crBu = Vec2.cross(m_rB, m_u) + var invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu + + // Compute the effective mass matrix. + m_mass = if (invMass != 0.0f) 1.0f / invMass else 0.0f + + if (frequency > 0.0f) { + val C = length - this.length + + // Frequency + val omega = 2.0f * MathUtils.PI * frequency + + // Damping coefficient + val d = 2.0f * m_mass * dampingRatio * omega + + // Spring stiffness + val k = m_mass * omega * omega + + // magic formulas + val h = data.step!!.dt + m_gamma = h * (d + h * k) + m_gamma = if (m_gamma != 0.0f) 1.0f / m_gamma else 0.0f + m_bias = C * h * k * m_gamma + + invMass += m_gamma + m_mass = if (invMass != 0.0f) 1.0f / invMass else 0.0f + } else { + m_gamma = 0.0f + m_bias = 0.0f + } + if (data.step!!.warmStarting) { + + // Scale the impulse to support a variable time step. + m_impulse *= data.step!!.dtRatio + + val P = pool.popVec2() + P.set(m_u).mulLocal(m_impulse) + + vA.x -= m_invMassA * P.x + vA.y -= m_invMassA * P.y + wA -= m_invIA * Vec2.cross(m_rA, P) + + vB.x += m_invMassB * P.x + vB.y += m_invMassB * P.y + wB += m_invIB * Vec2.cross(m_rB, P) + + pool.pushVec2(1) + } else { + m_impulse = 0.0f + } + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val vpA = pool.popVec2() + val vpB = pool.popVec2() + + // Cdot = dot(u, v + cross(w, r)) + Vec2.crossToOutUnsafe(wA, m_rA, vpA) + vpA.addLocal(vA) + Vec2.crossToOutUnsafe(wB, m_rB, vpB) + vpB.addLocal(vB) + val Cdot = Vec2.dot(m_u, vpB.subLocal(vpA)) + + val impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse) + m_impulse += impulse + + + val Px = impulse * m_u.x + val Py = impulse * m_u.y + + vA.x -= m_invMassA * Px + vA.y -= m_invMassA * Py + wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px) + vB.x += m_invMassB * Px + vB.y += m_invMassB * Py + wB += m_invIB * (m_rB.x * Py - m_rB.y * Px) + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(2) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + if (frequency > 0.0f) { + return true + } + val qA = pool.popRot() + val qB = pool.popRot() + val rA = pool.popVec2() + val rB = pool.popVec2() + val u = pool.popVec2() + + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + qA.setRadians(aA) + qB.setRadians(aB) + + Rot.mulToOutUnsafe(qA, u.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, u.set(localAnchorB).subLocal(m_localCenterB), rB) + u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + + + val length = u.normalize() + var C = length - this.length + C = MathUtils.clamp(C, -Settings.maxLinearCorrection, Settings.maxLinearCorrection) + + val impulse = -m_mass * C + val Px = impulse * u.x + val Py = impulse * u.y + + cA.x -= m_invMassA * Px + cA.y -= m_invMassA * Py + aA -= m_invIA * (rA.x * Py - rA.y * Px) + cB.x += m_invMassB * Px + cB.y += m_invMassB * Py + aB += m_invIB * (rB.x * Py - rB.y * Px) + + // data.positions[m_indexA].c.set(cA); + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c.set(cB); + data.positions!![m_indexB].a = aB + + pool.pushVec2(3) + pool.pushRot(2) + + return MathUtils.abs(C) < Settings.linearSlop + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJointDef.kt new file mode 100644 index 0000000..067a46f --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/DistanceJointDef.kt @@ -0,0 +1,102 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +//Updated to rev 56->130->142 of b2DistanceJoint.cpp/.h + +/** + * Distance joint definition. This requires defining an anchor point on both bodies and the non-zero + * length of the distance joint. The definition uses local anchor points so that the initial + * configuration can violate the constraint slightly. This helps when saving and loading a game. + * + * @warning Do not use a zero or short length. + */ +class DistanceJointDef : JointDef(JointType.DISTANCE) { + /** The local anchor point relative to body1's origin. */ + + val localAnchorA: Vec2 = Vec2(0.0f, 0.0f) + + /** The local anchor point relative to body2's origin. */ + + val localAnchorB: Vec2 = Vec2(0.0f, 0.0f) + + /** The equilibrium length between the anchor points. */ + + var length: Float = 1f + + /** + * The mass-spring-damper frequency in Hertz. + */ + + var frequencyHz: Float = 0f + + /** + * The damping ratio. 0 = no damping, 1 = critical damping. + */ + + var dampingRatio: Float = 0f + + /** + * Initialize the bodies, anchors, and length using the world anchors. + * + * @param b1 First body + * @param b2 Second body + * @param anchor1 World anchor on first body + * @param anchor2 World anchor on second body + */ + fun initialize(b1: Body, b2: Body, anchor1: Vec2, anchor2: Vec2) { + bodyA = b1 + bodyB = b2 + localAnchorA.set(bodyA!!.getLocalPoint(anchor1)) + localAnchorB.set(bodyB!!.getLocalPoint(anchor2)) + val d = anchor2.sub(anchor1) + length = d.length() + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJoint.kt new file mode 100644 index 0000000..248a9b5 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJoint.kt @@ -0,0 +1,278 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 7:27:32 AM Jan 20, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Mat22 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +/** + * @author Daniel Murphy + */ +class FrictionJoint(argWorldPool: IWorldPool, def: FrictionJointDef) : Joint(argWorldPool, def) { + + val localAnchorA: Vec2 = Vec2(def.localAnchorA) + + val localAnchorB: Vec2 = Vec2(def.localAnchorB) + + // Solver shared + private val m_linearImpulse: Vec2 + private var m_angularImpulse: Float = 0.toFloat() + private var m_maxForce: Float = 0.toFloat() + private var m_maxTorque: Float = 0.toFloat() + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_linearMass = Mat22() + private var m_angularMass: Float = 0.toFloat() + + var maxForce: Float + get() = m_maxForce + set(force) { + assert(force >= 0.0f) + m_maxForce = force + } + + var maxTorque: Float + get() = m_maxTorque + set(torque) { + assert(torque >= 0.0f) + m_maxTorque = torque + } + + init { + + m_linearImpulse = Vec2() + m_angularImpulse = 0.0f + + m_maxForce = def.maxForce + m_maxTorque = def.maxTorque + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_linearImpulse).mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return inv_dt * m_angularImpulse + } + + /** + * @see org.jbox2d.dynamics.joints.Joint.initVelocityConstraints + */ + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + + val temp = pool.popVec2() + val qA = pool.popRot() + val qB = pool.popRot() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective mass matrix. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), m_rB) + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val K = pool.popMat22() + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y + K.ey.x = K.ex.y + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x + + K.invertToOut(m_linearMass) + + m_angularMass = iA + iB + if (m_angularMass > 0.0f) { + m_angularMass = 1.0f / m_angularMass + } + + if (data.step!!.warmStarting) { + // Scale impulses to support a variable time step. + m_linearImpulse.mulLocal(data.step!!.dtRatio) + m_angularImpulse *= data.step!!.dtRatio + + val P = pool.popVec2() + P.set(m_linearImpulse) + + temp.set(P).mulLocal(mA) + vA.subLocal(temp) + wA -= iA * (Vec2.cross(m_rA, P) + m_angularImpulse) + + temp.set(P).mulLocal(mB) + vB.addLocal(temp) + wB += iB * (Vec2.cross(m_rB, P) + m_angularImpulse) + + pool.pushVec2(1) + } else { + m_linearImpulse.setZero() + m_angularImpulse = 0.0f + } + // data.velocities[m_indexA].v.set(vA); + if (data.velocities!![m_indexA].w != wA) { + assert(data.velocities!![m_indexA].w != wA) + } + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushRot(2) + pool.pushVec2(1) + pool.pushMat22(1) + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val h = data.step!!.dt + + // Solve angular friction + run { + val Cdot = wB - wA + var impulse = -m_angularMass * Cdot + + val oldImpulse = m_angularImpulse + val maxImpulse = h * m_maxTorque + m_angularImpulse = MathUtils.clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse) + impulse = m_angularImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + // Solve linear friction + run { + val Cdot = pool.popVec2() + val temp = pool.popVec2() + + Vec2.crossToOutUnsafe(wA, m_rA, temp) + Vec2.crossToOutUnsafe(wB, m_rB, Cdot) + Cdot.addLocal(vB).subLocal(vA).subLocal(temp) + + val impulse = pool.popVec2() + Mat22.mulToOutUnsafe(m_linearMass, Cdot, impulse) + impulse.negateLocal() + + + val oldImpulse = pool.popVec2() + oldImpulse.set(m_linearImpulse) + m_linearImpulse.addLocal(impulse) + + val maxImpulse = h * m_maxForce + + if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { + m_linearImpulse.normalize() + m_linearImpulse.mulLocal(maxImpulse) + } + + impulse.set(m_linearImpulse).subLocal(oldImpulse) + + temp.set(impulse).mulLocal(mA) + vA.subLocal(temp) + wA -= iA * Vec2.cross(m_rA, impulse) + + temp.set(impulse).mulLocal(mB) + vB.addLocal(temp) + wB += iB * Vec2.cross(m_rB, impulse) + + } + + // data.velocities[m_indexA].v.set(vA); + if (data.velocities!![m_indexA].w != wA) { + assert(data.velocities!![m_indexA].w != wA) + } + data.velocities!![m_indexA].w = wA + + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(4) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + return true + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJointDef.kt new file mode 100644 index 0000000..016227b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/FrictionJointDef.kt @@ -0,0 +1,74 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 7:23:39 AM Jan 20, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Friction joint definition. + * + * @author Daniel Murphy + */ +class FrictionJointDef : JointDef(JointType.FRICTION) { + + + /** + * The local anchor point relative to bodyA's origin. + */ + + val localAnchorA: Vec2 = Vec2() + + /** + * The local anchor point relative to bodyB's origin. + */ + + val localAnchorB: Vec2 = Vec2() + + /** + * The maximum friction force in N. + */ + + var maxForce: Float = 0f + + /** + * The maximum friction torque in N-m. + */ + + var maxTorque: Float = 0f + + /** + * Initialize the bodies, anchors, axis, and reference angle using the world anchor and world + * axis. + */ + fun initialize(bA: Body, bB: Body, anchor: Vec2) { + bodyA = bA + bodyB = bB + bA.getLocalPointToOut(anchor, localAnchorA) + bB.getLocalPointToOut(anchor, localAnchorB) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJoint.kt new file mode 100644 index 0000000..b80d050 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJoint.kt @@ -0,0 +1,520 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 11:34:45 AM Jan 23, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +//Gear Joint: +//C0 = (coordinate1 + ratio * coordinate2)_initial +//C = (coordinate1 + ratio * coordinate2) - C0 = 0 +//J = [J1 ratio * J2] +//K = J * invM * JT +//= J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T +// +//Revolute: +//coordinate = rotation +//Cdot = angularVelocity +//J = [0 0 1] +//K = J * invM * JT = invI +// +//Prismatic: +//coordinate = dot(p - pg, ug) +//Cdot = dot(v + cross(w, r), ug) +//J = [ug cross(r, ug)] +//K = J * invM * JT = invMass + invI * cross(r, ug)^2 + +/** + * A gear joint is used to connect two joints together. Either joint can be a revolute or prismatic + * joint. You specify a gear ratio to bind the motions together: coordinate1 + ratio * coordinate2 = + * constant The ratio can be negative or positive. If one joint is a revolute joint and the other + * joint is a prismatic joint, then the ratio will have units of length or units of 1/length. + * + * @warning The revolute and prismatic joints must be attached to fixed bodies (which must be body1 + * on those joints). + * @warning You have to manually destroy the gear joint if joint1 or joint2 is destroyed. + * @author Daniel Murphy + */ +class GearJoint(argWorldPool: IWorldPool, def: GearJointDef) : Joint(argWorldPool, def) { + + val joint1: Joint? = def.joint1 + + val joint2: Joint? = def.joint2 + + private val m_typeA: JointType + private val m_typeB: JointType + + // Body A is connected to body C + // Body B is connected to body D + private val m_bodyC: Body + private val m_bodyD: Body + + // Solver shared + private val m_localAnchorA = Vec2() + private val m_localAnchorB = Vec2() + private val m_localAnchorC = Vec2() + private val m_localAnchorD = Vec2() + + private val m_localAxisC = Vec2() + private val m_localAxisD = Vec2() + + private var m_referenceAngleA: Float = 0.toFloat() + private var m_referenceAngleB: Float = 0.toFloat() + + private val m_constant: Float + + var ratio: Float = 0.toFloat() + + private var m_impulse: Float = 0.toFloat() + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private var m_indexC: Int = 0 + private var m_indexD: Int = 0 + private val m_lcA = Vec2() + private val m_lcB = Vec2() + private val m_lcC = Vec2() + private val m_lcD = Vec2() + private var m_mA: Float = 0.toFloat() + private var m_mB: Float = 0.toFloat() + private var m_mC: Float = 0.toFloat() + private var m_mD: Float = 0.toFloat() + private var m_iA: Float = 0.toFloat() + private var m_iB: Float = 0.toFloat() + private var m_iC: Float = 0.toFloat() + private var m_iD: Float = 0.toFloat() + private val m_JvAC = Vec2() + private val m_JvBD = Vec2() + private var m_JwA: Float = 0.toFloat() + private var m_JwB: Float = 0.toFloat() + private var m_JwC: Float = 0.toFloat() + private var m_JwD: Float = 0.toFloat() + private var m_mass: Float = 0.toFloat() + + init { + + m_typeA = joint1!!.type + m_typeB = joint2!!.type + + assert(m_typeA === JointType.REVOLUTE || m_typeA === JointType.PRISMATIC) + assert(m_typeB === JointType.REVOLUTE || m_typeB === JointType.PRISMATIC) + + val coordinateA: Float + val coordinateB: Float + + // TODO_ERIN there might be some problem with the joint edges in Joint. + + m_bodyC = joint1.bodyA!! + bodyA = joint1.bodyB + + // Get geometry of joint1 + val xfA = bodyA!!.xf + val aA = bodyA!!.sweep.a + val xfC = m_bodyC.xf + val aC = m_bodyC.sweep.a + + if (m_typeA === JointType.REVOLUTE) { + val revolute = def.joint1 as RevoluteJoint? + m_localAnchorC.set(revolute!!.m_localAnchorA) + m_localAnchorA.set(revolute.m_localAnchorB) + m_referenceAngleA = revolute.m_referenceAngleRadians + m_localAxisC.setZero() + + coordinateA = aA - aC - m_referenceAngleA + } else { + val pA = pool.popVec2() + val temp = pool.popVec2() + val prismatic = def.joint1 as PrismaticJoint? + m_localAnchorC.set(prismatic!!.m_localAnchorA) + m_localAnchorA.set(prismatic.m_localAnchorB) + m_referenceAngleA = prismatic.m_referenceAngleRadians + m_localAxisC.set(prismatic.m_localXAxisA) + + val pC = m_localAnchorC + Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp) + temp.addLocal(xfA.p).subLocal(xfC.p) + Rot.mulTransUnsafe(xfC.q, temp, pA) + coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC) + pool.pushVec2(2) + } + + m_bodyD = joint2.bodyA!! + bodyB = joint2.bodyB + + // Get geometry of joint2 + val xfB = bodyB!!.xf + val aB = bodyB!!.sweep.a + val xfD = m_bodyD.xf + val aD = m_bodyD.sweep.a + + if (m_typeB === JointType.REVOLUTE) { + val revolute = def.joint2 as RevoluteJoint? + m_localAnchorD.set(revolute!!.m_localAnchorA) + m_localAnchorB.set(revolute.m_localAnchorB) + m_referenceAngleB = revolute.m_referenceAngleRadians + m_localAxisD.setZero() + + coordinateB = aB - aD - m_referenceAngleB + } else { + val pB = pool.popVec2() + val temp = pool.popVec2() + val prismatic = def.joint2 as PrismaticJoint? + m_localAnchorD.set(prismatic!!.m_localAnchorA) + m_localAnchorB.set(prismatic.m_localAnchorB) + m_referenceAngleB = prismatic.m_referenceAngleRadians + m_localAxisD.set(prismatic.m_localXAxisA) + + val pD = m_localAnchorD + Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp) + temp.addLocal(xfB.p).subLocal(xfD.p) + Rot.mulTransUnsafe(xfD.q, temp, pB) + coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD) + pool.pushVec2(2) + } + + ratio = def.ratio + + m_constant = coordinateA + ratio * coordinateB + + m_impulse = 0.0f + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(m_localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(m_localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_JvAC).mulLocal(m_impulse) + argOut.mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + val L = m_impulse * m_JwA + return inv_dt * L + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_indexC = m_bodyC.islandIndex + m_indexD = m_bodyD.islandIndex + m_lcA.set(bodyA!!.sweep.localCenter) + m_lcB.set(bodyB!!.sweep.localCenter) + m_lcC.set(m_bodyC.sweep.localCenter) + m_lcD.set(m_bodyD.sweep.localCenter) + m_mA = bodyA!!.m_invMass + m_mB = bodyB!!.m_invMass + m_mC = m_bodyC.m_invMass + m_mD = m_bodyD.m_invMass + m_iA = bodyA!!.m_invI + m_iB = bodyB!!.m_invI + m_iC = m_bodyC.m_invI + m_iD = m_bodyD.m_invI + + // Vec2 cA = data.positions[m_indexA].c; + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + // Vec2 cB = data.positions[m_indexB].c; + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + // Vec2 cC = data.positions[m_indexC].c; + val aC = data.positions!![m_indexC].a + val vC = data.velocities!![m_indexC].v + var wC = data.velocities!![m_indexC].w + + // Vec2 cD = data.positions[m_indexD].c; + val aD = data.positions!![m_indexD].a + val vD = data.velocities!![m_indexD].v + var wD = data.velocities!![m_indexD].w + + val qA = pool.popRot() + val qB = pool.popRot() + val qC = pool.popRot() + val qD = pool.popRot() + qA.setRadians(aA) + qB.setRadians(aB) + qC.setRadians(aC) + qD.setRadians(aD) + + m_mass = 0.0f + + val temp = pool.popVec2() + + if (m_typeA === JointType.REVOLUTE) { + m_JvAC.setZero() + m_JwA = 1.0f + m_JwC = 1.0f + m_mass += m_iA + m_iC + } else { + val rC = pool.popVec2() + val rA = pool.popVec2() + Rot.mulToOutUnsafe(qC, m_localAxisC, m_JvAC) + Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC) + Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA) + m_JwC = Vec2.cross(rC, m_JvAC) + m_JwA = Vec2.cross(rA, m_JvAC) + m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA + pool.pushVec2(2) + } + + if (m_typeB === JointType.REVOLUTE) { + m_JvBD.setZero() + m_JwB = ratio + m_JwD = ratio + m_mass += ratio * ratio * (m_iB + m_iD) + } else { + val u = pool.popVec2() + val rD = pool.popVec2() + val rB = pool.popVec2() + Rot.mulToOutUnsafe(qD, m_localAxisD, u) + Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD) + Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB) + m_JvBD.set(u).mulLocal(ratio) + m_JwD = ratio * Vec2.cross(rD, u) + m_JwB = ratio * Vec2.cross(rB, u) + m_mass += ratio * ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB + pool.pushVec2(3) + } + + // Compute effective mass. + m_mass = if (m_mass > 0.0f) 1.0f / m_mass else 0.0f + + if (data.step!!.warmStarting) { + vA.x += m_mA * m_impulse * m_JvAC.x + vA.y += m_mA * m_impulse * m_JvAC.y + wA += m_iA * m_impulse * m_JwA + + vB.x += m_mB * m_impulse * m_JvBD.x + vB.y += m_mB * m_impulse * m_JvBD.y + wB += m_iB * m_impulse * m_JwB + + vC.x -= m_mC * m_impulse * m_JvAC.x + vC.y -= m_mC * m_impulse * m_JvAC.y + wC -= m_iC * m_impulse * m_JwC + + vD.x -= m_mD * m_impulse * m_JvBD.x + vD.y -= m_mD * m_impulse * m_JvBD.y + wD -= m_iD * m_impulse * m_JwD + } else { + m_impulse = 0.0f + } + pool.pushVec2(1) + pool.pushRot(4) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + // data.velocities[m_indexC].v = vC; + data.velocities!![m_indexC].w = wC + // data.velocities[m_indexD].v = vD; + data.velocities!![m_indexD].w = wD + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + val vC = data.velocities!![m_indexC].v + var wC = data.velocities!![m_indexC].w + val vD = data.velocities!![m_indexD].v + var wD = data.velocities!![m_indexD].w + + val temp1 = pool.popVec2() + val temp2 = pool.popVec2() + var Cdot = Vec2.dot(m_JvAC, temp1.set(vA).subLocal(vC)) + Vec2.dot(m_JvBD, temp2.set(vB).subLocal(vD)) + Cdot += m_JwA * wA - m_JwC * wC + (m_JwB * wB - m_JwD * wD) + pool.pushVec2(2) + + val impulse = -m_mass * Cdot + m_impulse += impulse + + vA.x += m_mA * impulse * m_JvAC.x + vA.y += m_mA * impulse * m_JvAC.y + wA += m_iA * impulse * m_JwA + + vB.x += m_mB * impulse * m_JvBD.x + vB.y += m_mB * impulse * m_JvBD.y + wB += m_iB * impulse * m_JwB + + vC.x -= m_mC * impulse * m_JvAC.x + vC.y -= m_mC * impulse * m_JvAC.y + wC -= m_iC * impulse * m_JwC + + vD.x -= m_mD * impulse * m_JvBD.x + vD.y -= m_mD * impulse * m_JvBD.y + wD -= m_iD * impulse * m_JwD + + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + // data.velocities[m_indexC].v = vC; + data.velocities!![m_indexC].w = wC + // data.velocities[m_indexD].v = vD; + data.velocities!![m_indexD].w = wD + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + val cC = data.positions!![m_indexC].c + var aC = data.positions!![m_indexC].a + val cD = data.positions!![m_indexD].c + var aD = data.positions!![m_indexD].a + + val qA = pool.popRot() + val qB = pool.popRot() + val qC = pool.popRot() + val qD = pool.popRot() + qA.setRadians(aA) + qB.setRadians(aB) + qC.setRadians(aC) + qD.setRadians(aD) + + val linearError = 0.0f + + val coordinateA: Float + val coordinateB: Float + + val temp = pool.popVec2() + val JvAC = pool.popVec2() + val JvBD = pool.popVec2() + val JwA: Float + val JwB: Float + val JwC: Float + val JwD: Float + var mass = 0.0f + + if (m_typeA === JointType.REVOLUTE) { + JvAC.setZero() + JwA = 1.0f + JwC = 1.0f + mass += m_iA + m_iC + + coordinateA = aA - aC - m_referenceAngleA + } else { + val rC = pool.popVec2() + val rA = pool.popVec2() + val pC = pool.popVec2() + val pA = pool.popVec2() + Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC) + Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC) + Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA) + JwC = Vec2.cross(rC, JvAC) + JwA = Vec2.cross(rA, JvAC) + mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA + + pC.set(m_localAnchorC).subLocal(m_lcC) + Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA) + coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC) + pool.pushVec2(4) + } + + if (m_typeB === JointType.REVOLUTE) { + JvBD.setZero() + JwB = ratio + JwD = ratio + mass += ratio * ratio * (m_iB + m_iD) + + coordinateB = aB - aD - m_referenceAngleB + } else { + val u = pool.popVec2() + val rD = pool.popVec2() + val rB = pool.popVec2() + val pD = pool.popVec2() + val pB = pool.popVec2() + Rot.mulToOutUnsafe(qD, m_localAxisD, u) + Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD) + Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB) + JvBD.set(u).mulLocal(ratio) + JwD = Vec2.cross(rD, u) + JwB = Vec2.cross(rB, u) + mass += ratio * ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB + + pD.set(m_localAnchorD).subLocal(m_lcD) + Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB) + coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD) + pool.pushVec2(5) + } + + val C = coordinateA + ratio * coordinateB - m_constant + + var impulse = 0.0f + if (mass > 0.0f) { + impulse = -C / mass + } + pool.pushVec2(3) + pool.pushRot(4) + + cA.x += m_mA * impulse * JvAC.x + cA.y += m_mA * impulse * JvAC.y + aA += m_iA * impulse * JwA + + cB.x += m_mB * impulse * JvBD.x + cB.y += m_mB * impulse * JvBD.y + aB += m_iB * impulse * JwB + + cC.x -= m_mC * impulse * JvAC.x + cC.y -= m_mC * impulse * JvAC.y + aC -= m_iC * impulse * JwC + + cD.x -= m_mD * impulse * JvBD.x + cD.y -= m_mD * impulse * JvBD.y + aD -= m_iD * impulse * JwD + + // data.positions[m_indexA].c = cA; + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c = cB; + data.positions!![m_indexB].a = aB + // data.positions[m_indexC].c = cC; + data.positions!![m_indexC].a = aC + // data.positions[m_indexD].c = cD; + data.positions!![m_indexD].a = aD + + // TODO_ERIN not implemented + return linearError < Settings.linearSlop + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJointDef.kt new file mode 100644 index 0000000..1ee2d95 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/GearJointDef.kt @@ -0,0 +1,55 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 5:20:39 AM Jan 22, 2011 + */ +package org.jbox2d.dynamics.joints + +/** + * Gear joint definition. This definition requires two existing revolute or prismatic joints (any + * combination will work). The provided joints must attach a dynamic body to a static body. + * + * @author Daniel Murphy + */ +class GearJointDef : JointDef(JointType.GEAR) { + /** + * The first revolute/prismatic joint attached to the gear joint. + */ + + var joint1: Joint? = null + + /** + * The second revolute/prismatic joint attached to the gear joint. + */ + + var joint2: Joint? = null + + /** + * Gear ratio. + * + * @see GearJoint + */ + + var ratio: Float = 0f +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Jacobian.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Jacobian.kt new file mode 100644 index 0000000..238023b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Jacobian.kt @@ -0,0 +1,35 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 + +class Jacobian { + + val linearA = Vec2() + + var angularA: Float = 0f + + var angularB: Float = 0f +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Joint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Joint.kt new file mode 100644 index 0000000..668601f --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/Joint.kt @@ -0,0 +1,182 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body +import org.jbox2d.dynamics.SolverData +import org.jbox2d.dynamics.World +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool +import org.jbox2d.userdata.Box2dTypedUserData + +/** + * The base joint class. Joints are used to constrain two bodies together in various fashions. Some + * joints also feature limits and motors. + * + * @author Daniel Murphy + */ +abstract class Joint protected constructor( + // Cache here per time step to reduce cache misses. + protected var pool: IWorldPool, + def: JointDef +) : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + + /** + * get the type of the concrete joint. + * + * @return + */ + val type: JointType + + var prev: Joint? = null + + /** + * get the next joint the world joint list. + */ + var next: Joint? = null + + var edgeA: JointEdge + var edgeB: JointEdge + + /** + * get the first body attached to this joint. + */ + var bodyA: Body? = null + + /** + * get the second body attached to this joint. + */ + var bodyB: Body? = null + + var islandFlag: Boolean = false + + /** + * Get collide connected. Note: modifying the collide connect flag won't work correctly because + * the flag is only checked when fixture AABBs begin to overlap. + */ + + val _collideConnected: Boolean + + fun getCollideConnected() = _collideConnected + + var userData: Any? = null + + /** + * Short-cut function to determine if either body is inactive. + */ + val isActive: Boolean + get() = bodyA!!.isActive && bodyB!!.isActive + + init { + assert(def.bodyA !== def.bodyB) + this.type = def.type + prev = null + next = null + bodyA = def.bodyA + bodyB = def.bodyB + _collideConnected = def.collideConnected + islandFlag = false + userData = def.userData + + edgeA = JointEdge() + edgeA.joint = null + edgeA.other = null + edgeA.prev = null + edgeA.next = null + + edgeB = JointEdge() + edgeB.joint = null + edgeB.other = null + edgeB.prev = null + edgeB.next = null + + // m_localCenterA = new Vec2(); + // m_localCenterB = new Vec2(); + } + + /** + * get the anchor point on bodyA in world coordinates. + */ + abstract fun getAnchorA(out: Vec2) + + /** + * get the anchor point on bodyB in world coordinates. + */ + abstract fun getAnchorB(out: Vec2) + + /** + * get the reaction force on body2 at the joint anchor in Newtons. + */ + abstract fun getReactionForce(inv_dt: Float, out: Vec2) + + /** + * get the reaction torque on body2 in N*m. + */ + abstract fun getReactionTorque(inv_dt: Float): Float + + /** Internal */ + abstract fun initVelocityConstraints(data: SolverData) + + /** Internal */ + abstract fun solveVelocityConstraints(data: SolverData) + + /** + * This returns true if the position errors are within tolerance. Internal. + */ + abstract fun solvePositionConstraints(data: SolverData): Boolean + + /** + * Override to handle destruction of joint + */ + open fun destructor() {} + + companion object { + + fun create(world: World, def: JointDef): Joint? { + // Joint joint = null; + when (def.type) { + JointType.MOUSE -> return MouseJoint(world.pool, def as MouseJointDef) + JointType.DISTANCE -> return DistanceJoint(world.pool, def as DistanceJointDef) + JointType.PRISMATIC -> return PrismaticJoint(world.pool, def as PrismaticJointDef) + JointType.REVOLUTE -> return RevoluteJoint(world.pool, def as RevoluteJointDef) + JointType.WELD -> return WeldJoint(world.pool, def as WeldJointDef) + JointType.FRICTION -> return FrictionJoint(world.pool, def as FrictionJointDef) + JointType.WHEEL -> return WheelJoint(world.pool, def as WheelJointDef) + JointType.GEAR -> return GearJoint(world.pool, def as GearJointDef) + JointType.PULLEY -> return PulleyJoint(world.pool, def as PulleyJointDef) + JointType.CONSTANT_VOLUME -> return ConstantVolumeJoint(world, def as ConstantVolumeJointDef) + JointType.ROPE -> return RopeJoint(world.pool, def as RopeJointDef) + JointType.MOTOR -> return MotorJoint(world.pool, def as MotorJointDef) + JointType.UNKNOWN -> return null + else -> return null + } + } + + + fun destroy(joint: Joint) { + joint.destructor() + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointDef.kt new file mode 100644 index 0000000..bc3d775 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointDef.kt @@ -0,0 +1,63 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.dynamics.Body +import org.jbox2d.userdata.Box2dTypedUserData + +/** + * Joint definitions are used to construct joints. + * @author Daniel Murphy + */ +open class JointDef( + /** + * The joint type is set automatically for concrete joint types. + */ + + var type: JointType) : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + + /** + * Use this to attach application specific data to your joints. + */ + + var userData: Any? = null + + /** + * The first attached body. + */ + + var bodyA: Body? = null + + /** + * The second attached body. + */ + + var bodyB: Body? = null + + /** + * Set this flag to true if the attached bodies should collide. + */ + + var collideConnected: Boolean = false +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointEdge.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointEdge.kt new file mode 100644 index 0000000..0a9617d --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointEdge.kt @@ -0,0 +1,61 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.dynamics.Body + +/** + * A joint edge is used to connect bodies and joints together + * in a joint graph where each body is a node and each joint + * is an edge. A joint edge belongs to a doubly linked list + * maintained in each attached body. Each joint has two joint + * nodes, one for each attached body. + * @author Daniel + */ +class JointEdge { + + /** + * Provides quick access to the other body attached + */ + + var other: Body? = null + + /** + * the joint + */ + + var joint: Joint? = null + + /** + * the previous joint edge in the body's joint list + */ + + var prev: JointEdge? = null + + /** + * the next joint edge in the body's joint list + */ + + var next: JointEdge? = null +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointType.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointType.kt new file mode 100644 index 0000000..03fa236 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/JointType.kt @@ -0,0 +1,28 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +enum class JointType { + UNKNOWN, REVOLUTE, PRISMATIC, DISTANCE, PULLEY, MOUSE, GEAR, WHEEL, WELD, FRICTION, ROPE, CONSTANT_VOLUME, MOTOR +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/LimitState.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/LimitState.kt new file mode 100644 index 0000000..67f697e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/LimitState.kt @@ -0,0 +1,28 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +enum class LimitState { + INACTIVE, AT_LOWER, AT_UPPER, EQUAL +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJoint.kt new file mode 100644 index 0000000..f13f509 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJoint.kt @@ -0,0 +1,311 @@ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Mat22 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.* +import org.jbox2d.pooling.IWorldPool + +//Point-to-point constraint +//Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +//J = [-I -r1_skew I r2_skew ] +//Identity used: +//w k % (rx i + ry j) = w * (-ry i + rx j) + +//Angle constraint +//Cdot = w2 - w1 +//J = [0 0 -1 0 0 1] +//K = invI1 + invI2 + +/** + * A motor joint is used to control the relative motion between two bodies. A typical usage is to + * control the movement of a dynamic body with respect to the ground. + * + * @author dmurph + */ +class MotorJoint(pool: IWorldPool, def: MotorJointDef) : Joint(pool, def) { + + // Solver shared + /** + * Get the target linear offset, in frame A, in meters. Do not modify. + */ + /** + * Set the target linear offset, in frame A, in meters. + */ + var linearOffset = Vec2() + set(linearOffset) { + if (linearOffset.x != this.linearOffset.x || linearOffset.y != this.linearOffset.y) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + this.linearOffset.set(linearOffset) + } + } + private var m_angularOffset: Float = def.angularOffset + private val m_linearImpulse = Vec2() + private var m_angularImpulse: Float = 0f + private var m_maxForce: Float = def.maxForce + private var m_maxTorque: Float = def.maxTorque + + var correctionFactor: Float = def.correctionFactor + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private val m_linearError = Vec2() + private var m_angularError: Float = 0.toFloat() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_linearMass = Mat22() + private var m_angularMass: Float = 0.toFloat() + + /** + * Set the target angular offset, in radians. + * + * @param angularOffset + */ + var angularOffset: Float + get() = m_angularOffset + set(angularOffset) { + if (angularOffset != m_angularOffset) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_angularOffset = angularOffset + } + } + + /** + * Get the maximum friction force in N. + */ + /** + * Set the maximum friction force in N. + * + * @param force + */ + var maxForce: Float + get() = m_maxForce + set(force) { + assert(force >= 0.0f) + m_maxForce = force + } + + /** + * Get the maximum friction torque in N*m. + */ + /** + * Set the maximum friction torque in N*m. + */ + var maxTorque: Float + get() = m_maxTorque + set(torque) { + assert(torque >= 0.0f) + m_maxTorque = torque + } + + init { + linearOffset.set(def.linearOffset) + } + + override fun getAnchorA(out: Vec2) { + out.set(bodyA!!.position) + } + + override fun getAnchorB(out: Vec2) { + out.set(bodyB!!.position) + } + + override fun getReactionForce(inv_dt: Float, out: Vec2) { + out.set(m_linearImpulse).mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return m_angularImpulse * inv_dt + } + + /** + * Get the target linear offset, in frame A, in meters. + */ + fun getLinearOffset(out: Vec2) { + out.set(linearOffset) + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + val K = pool.popMat22() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective mass matrix. + // m_rA = b2Mul(qA, -m_localCenterA); + // m_rB = b2Mul(qB, -m_localCenterB); + m_rA.x = qA.c * -m_localCenterA.x - qA.s * -m_localCenterA.y + m_rA.y = qA.s * -m_localCenterA.x + qA.c * -m_localCenterA.y + m_rB.x = qB.c * -m_localCenterB.x - qB.s * -m_localCenterB.y + m_rB.y = qB.s * -m_localCenterB.x + qB.c * -m_localCenterB.y + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y + K.ey.x = K.ex.y + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x + + K.invertToOut(m_linearMass) + + m_angularMass = iA + iB + if (m_angularMass > 0.0f) { + m_angularMass = 1.0f / m_angularMass + } + + // m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); + Rot.mulToOutUnsafe(qA, linearOffset, temp) + m_linearError.x = cB.x + m_rB.x - cA.x - m_rA.x - temp.x + m_linearError.y = cB.y + m_rB.y - cA.y - m_rA.y - temp.y + m_angularError = aB - aA - m_angularOffset + + if (data.step!!.warmStarting) { + // Scale impulses to support a variable time step. + m_linearImpulse.x *= data.step!!.dtRatio + m_linearImpulse.y *= data.step!!.dtRatio + m_angularImpulse *= data.step!!.dtRatio + + val P = m_linearImpulse + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * (m_rA.x * P.y - m_rA.y * P.x + m_angularImpulse) + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * (m_rB.x * P.y - m_rB.y * P.x + m_angularImpulse) + } else { + m_linearImpulse.setZero() + m_angularImpulse = 0.0f + } + + pool.pushVec2(1) + pool.pushMat22(1) + pool.pushRot(2) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val h = data.step!!.dt + val inv_h = data.step!!.inv_dt + + val temp = pool.popVec2() + + // Solve angular friction + run { + val Cdot = wB - wA + inv_h * correctionFactor * m_angularError + var impulse = -m_angularMass * Cdot + + val oldImpulse = m_angularImpulse + val maxImpulse = h * m_maxTorque + m_angularImpulse = MathUtils.clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse) + impulse = m_angularImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + val Cdot = pool.popVec2() + + // Solve linear friction + run { + // Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * + // m_linearError; + Cdot.x = vB.x + -wB * m_rB.y - vA.x - -wA * m_rA.y + inv_h * correctionFactor * m_linearError.x + Cdot.y = vB.y + wB * m_rB.x - vA.y - wA * m_rA.x + inv_h * correctionFactor * m_linearError.y + + val impulse = temp + Mat22.mulToOutUnsafe(m_linearMass, Cdot, impulse) + impulse.negateLocal() + val oldImpulse = pool.popVec2() + oldImpulse.set(m_linearImpulse) + m_linearImpulse.addLocal(impulse) + + val maxImpulse = h * m_maxForce + + if (m_linearImpulse.lengthSquared() > maxImpulse * maxImpulse) { + m_linearImpulse.normalize() + m_linearImpulse.mulLocal(maxImpulse) + } + + impulse.x = m_linearImpulse.x - oldImpulse.x + impulse.y = m_linearImpulse.y - oldImpulse.y + + vA.x -= mA * impulse.x + vA.y -= mA * impulse.y + wA -= iA * (m_rA.x * impulse.y - m_rA.y * impulse.x) + + vB.x += mB * impulse.x + vB.y += mB * impulse.y + wB += iB * (m_rB.x * impulse.y - m_rB.y * impulse.x) + } + + pool.pushVec2(3) + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + return true + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJointDef.kt new file mode 100644 index 0000000..75f4bad --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MotorJointDef.kt @@ -0,0 +1,52 @@ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Motor joint definition. + * + * @author dmurph + */ +class MotorJointDef : JointDef(JointType.MOTOR) { + /** + * Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. + */ + + val linearOffset = Vec2() + + /** + * The bodyB angle minus bodyA angle in radians. + */ + + var angularOffset: Float = 0f + + /** + * The maximum motor force in N. + */ + + var maxForce: Float = 1f + + /** + * The maximum motor torque in N-m. + */ + + var maxTorque: Float = 1f + + /** + * Position correction factor in the range [0,1]. + */ + + var correctionFactor: Float = .3f + + fun initialize(bA: Body, bB: Body) { + bodyA = bA + bodyB = bB + val xB = bodyB!!.position + bodyA!!.getLocalPointToOut(xB, linearOffset) + + val angleA = bodyA!!.angleRadians + val angleB = bodyB!!.angleRadians + angularOffset = angleB - angleA + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJoint.kt new file mode 100644 index 0000000..b735f4c --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJoint.kt @@ -0,0 +1,227 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Mat22 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +/** + * A mouse joint is used to make a point on a body track a specified world point. This a soft + * constraint with a maximum force. This allows the constraint to stretch and without applying huge + * forces. NOTE: this joint is not documented in the manual because it was developed to be used in + * the testbed. If you want to learn how to use the mouse joint, look at the testbed. + * + * @author Daniel + */ +class MouseJoint constructor(argWorld: IWorldPool, def: MouseJointDef) : Joint(argWorld, def) { + private val m_localAnchorB = Vec2() + var target = Vec2() + set(target) { + if (!bodyB!!.isAwake) { + bodyB!!.isAwake = true + } + this.target.set(target) + } + // / set/get the frequency in Hertz. + + var frequency: Float = 0.toFloat() + // / set/get the damping ratio (dimensionless). + + var dampingRatio: Float = 0.toFloat() + private var m_beta: Float = 0.toFloat() + + // Solver shared + private val m_impulse = Vec2() + // / set/get the maximum force in Newtons. + + var maxForce: Float = 0.toFloat() + private var m_gamma: Float = 0.toFloat() + + // Solver temp + private var m_indexB: Int = 0 + private val m_rB = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassB: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_mass = Mat22() + private val m_C = Vec2() + + init { + assert(def.target.isValid) + assert(def.maxForce >= 0) + assert(def.frequencyHz >= 0) + assert(def.dampingRatio >= 0) + + target.set(def.target) + Transform.mulTransToOutUnsafe(bodyB!!.xf, target, m_localAnchorB) + + maxForce = def.maxForce + m_impulse.setZero() + + frequency = def.frequencyHz + dampingRatio = def.dampingRatio + + m_beta = 0f + m_gamma = 0f + } + + override fun getAnchorA(argOut: Vec2) { + argOut.set(target) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(m_localAnchorB, argOut) + } + + override fun getReactionForce(invDt: Float, argOut: Vec2) { + argOut.set(m_impulse).mulLocal(invDt) + } + + override fun getReactionTorque(invDt: Float): Float { + return invDt * 0.0f + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexB = bodyB!!.islandIndex + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassB = bodyB!!.m_invMass + m_invIB = bodyB!!.m_invI + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qB = pool.popRot() + + qB.setRadians(aB) + + val mass = bodyB!!.m_mass + + // Frequency + val omega = 2.0f * MathUtils.PI * frequency + + // Damping coefficient + val d = 2.0f * mass * dampingRatio * omega + + // Spring stiffness + val k = mass * (omega * omega) + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + val h = data.step!!.dt + assert(d + h * k > Settings.EPSILON) + m_gamma = h * (d + h * k) + if (m_gamma != 0.0f) { + m_gamma = 1.0f / m_gamma + } + m_beta = h * k * m_gamma + + val temp = pool.popVec2() + + // Compute the effective mass matrix. + Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB) + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + val K = pool.popMat22() + K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma + K.ex.y = -m_invIB * m_rB.x * m_rB.y + K.ey.x = K.ex.y + K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma + + K.invertToOut(m_mass) + + m_C.set(cB).addLocal(m_rB).subLocal(target) + m_C.mulLocal(m_beta) + + // Cheat with some damping + wB *= 0.98f + + if (data.step!!.warmStarting) { + m_impulse.mulLocal(data.step!!.dtRatio) + vB.x += m_invMassB * m_impulse.x + vB.y += m_invMassB * m_impulse.y + wB += m_invIB * Vec2.cross(m_rB, m_impulse) + } else { + m_impulse.setZero() + } + + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(1) + pool.pushMat22(1) + pool.pushRot(1) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + return true + } + + override fun solveVelocityConstraints(data: SolverData) { + + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + // Cdot = v + cross(w, r) + val Cdot = pool.popVec2() + Vec2.crossToOutUnsafe(wB, m_rB, Cdot) + Cdot.addLocal(vB) + + val impulse = pool.popVec2() + val temp = pool.popVec2() + + temp.set(m_impulse).mulLocal(m_gamma).addLocal(m_C).addLocal(Cdot).negateLocal() + Mat22.mulToOutUnsafe(m_mass, temp, impulse) + + val oldImpulse = temp + oldImpulse.set(m_impulse) + m_impulse.addLocal(impulse) + val maxImpulse = data.step!!.dt * maxForce + if (m_impulse.lengthSquared() > maxImpulse * maxImpulse) { + m_impulse.mulLocal(maxImpulse / m_impulse.length()) + } + impulse.set(m_impulse).subLocal(oldImpulse) + + vB.x += m_invMassB * impulse.x + vB.y += m_invMassB * impulse.y + wB += m_invIB * Vec2.cross(m_rB, impulse) + + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(3) + } + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJointDef.kt new file mode 100644 index 0000000..5d51125 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/MouseJointDef.kt @@ -0,0 +1,58 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 + +/** + * Mouse joint definition. This requires a world target point, tuning parameters, and the time step. + * + * @author Daniel + */ +class MouseJointDef : JointDef(JointType.MOUSE) { + /** + * The initial world target point. This is assumed to coincide with the body anchor initially. + */ + + val target = Vec2(0f, 0f) + + /** + * The maximum constraint force that can be exerted to move the candidate body. Usually you will + * express as some multiple of the weight (multiplier * mass * gravity). + */ + + var maxForce: Float = 0f + + /** + * The response speed. + */ + + var frequencyHz: Float = .5f + + /** + * The damping ratio. 0 = no damping, 1 = critical damping. + */ + + var dampingRatio: Float = .7f +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJoint.kt new file mode 100644 index 0000000..e8548ff --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJoint.kt @@ -0,0 +1,785 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.* +import org.jbox2d.common.Mat33 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.* +import org.jbox2d.pooling.IWorldPool + +//Linear constraint (point-to-line) +//d = p2 - p1 = x2 + r2 - x1 - r1 +//C = dot(perp, d) +//Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +//J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] +// +//Angular constraint +//C = a2 - a1 + a_initial +//Cdot = w2 - w1 +//J = [0 0 -1 0 0 1] +// +//K = J * invM * JT +// +//J = [-a -s1 a s2] +// [0 -1 0 1] +//a = perp +//s1 = cross(d + r1, a) = cross(p2 - x1, a) +//s2 = cross(r2, a) = cross(p2 - x2, a) + + +//Motor/Limit linear constraint +//C = dot(ax1, d) +//Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) +//J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + +//Block Solver +//We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even +//when the mass has poor distribution (leading to large torques about the joint anchor points). +// +//The Jacobian has 3 rows: +//J = [-uT -s1 uT s2] // linear +// [0 -1 0 1] // angular +// [-vT -a1 vT a2] // limit +// +//u = perp +//v = axis +//s1 = cross(d + r1, u), s2 = cross(r2, u) +//a1 = cross(d + r1, v), a2 = cross(r2, v) + +//M * (v2 - v1) = JT * df +//J * v2 = bias +// +//v2 = v1 + invM * JT * df +//J * (v1 + invM * JT * df) = bias +//K * df = bias - J * v1 = -Cdot +//K = J * invM * JT +//Cdot = J * v1 - bias +// +//Now solve for f2. +//df = f2 - f1 +//K * (f2 - f1) = -Cdot +//f2 = invK * (-Cdot) + f1 +// +//Clamp accumulated limit impulse. +//lower: f2(3) = max(f2(3), 0) +//upper: f2(3) = min(f2(3), 0) +// +//Solve for correct f2(1:2) +//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 +// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) +//K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) +//f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) +// +//Now compute impulse to be applied: +//df = f2 - f1 + +/** + * A prismatic joint. This joint provides one degree of freedom: translation along an axis fixed in + * bodyA. Relative rotation is prevented. You can use a joint limit to restrict the range of motion + * and a joint motor to drive the motion or to model joint friction. + * + * @author Daniel + */ +class PrismaticJoint(argWorld: IWorldPool, def: PrismaticJointDef) : Joint(argWorld, def) { + + // Solver shared + + val m_localAnchorA: Vec2 = Vec2(def.localAnchorA) + + val m_localAnchorB: Vec2 = Vec2(def.localAnchorB) + + val m_localXAxisA: Vec2 = Vec2(def.localAxisA) + protected val m_localYAxisA: Vec2 = Vec2() + + var m_referenceAngleRadians: Float = 0.toFloat() + protected set + var m_referenceAngleDegrees: Float + protected set(value) { m_referenceAngleRadians = value * MathUtils.DEG2RAD } + get() = m_referenceAngleRadians * MathUtils.RAD2DEG + var m_referenceAngle: Angle + protected set(value) { m_referenceAngleRadians = value.radians.toFloat() } + get() = m_referenceAngleRadians.radians + + private val m_impulse: Vec3 = Vec3() + private var m_motorImpulse: Float = 0.toFloat() + /** + * Get the lower joint limit, usually in meters. + * + * @return + */ + var lowerLimit: Float = 0.toFloat() + private set + /** + * Get the upper joint limit, usually in meters. + * + * @return + */ + var upperLimit: Float = 0.toFloat() + private set + private var m_maxMotorForce: Float = 0.toFloat() + private var m_motorSpeed: Float = 0.toFloat() + /** + * Is the joint limit enabled? + * + * @return + */ + var isLimitEnabled: Boolean = false + private set + /** + * Is the joint motor enabled? + * + * @return + */ + var isMotorEnabled: Boolean = false + private set + private var m_limitState: LimitState? = null + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_axis: Vec2 + private val m_perp: Vec2 + private var m_s1: Float = 0.toFloat() + private var m_s2: Float = 0.toFloat() + private var m_a1: Float = 0.toFloat() + private var m_a2: Float = 0.toFloat() + private val m_K: Mat33 + private var m_motorMass: Float = 0.toFloat() // effective mass for motor/limit translational constraint. + + /** + * Get the current joint translation, usually in meters. + */ + val jointSpeed: Float + get() { + val bA = bodyA + val bB = bodyB + + val temp = pool.popVec2() + val rA = pool.popVec2() + val rB = pool.popVec2() + val p1 = pool.popVec2() + val p2 = pool.popVec2() + val d = pool.popVec2() + val axis = pool.popVec2() + val temp2 = pool.popVec2() + val temp3 = pool.popVec2() + + temp.set(m_localAnchorA).subLocal(bA!!.sweep.localCenter) + Rot.mulToOutUnsafe(bA.xf.q, temp, rA) + + temp.set(m_localAnchorB).subLocal(bB!!.sweep.localCenter) + Rot.mulToOutUnsafe(bB.xf.q, temp, rB) + + p1.set(bA.sweep.c).addLocal(rA) + p2.set(bB.sweep.c).addLocal(rB) + + d.set(p2).subLocal(p1) + Rot.mulToOutUnsafe(bA.xf.q, m_localXAxisA, axis) + + val vA = bA._linearVelocity + val vB = bB._linearVelocity + val wA = bA._angularVelocity + val wB = bB._angularVelocity + + + Vec2.crossToOutUnsafe(wA, axis, temp) + Vec2.crossToOutUnsafe(wB, rB, temp2) + Vec2.crossToOutUnsafe(wA, rA, temp3) + + temp2.addLocal(vB).subLocal(vA).subLocal(temp3) + val speed = Vec2.dot(d, temp) + Vec2.dot(axis, temp2) + + pool.pushVec2(9) + + return speed + } + + val jointTranslation: Float + get() { + val pA = pool.popVec2() + val pB = pool.popVec2() + val axis = pool.popVec2() + bodyA!!.getWorldPointToOut(m_localAnchorA, pA) + bodyB!!.getWorldPointToOut(m_localAnchorB, pB) + bodyA!!.getWorldVectorToOutUnsafe(m_localXAxisA, axis) + pB.subLocal(pA) + val translation = Vec2.dot(pB, axis) + pool.pushVec2(3) + return translation + } + + /** + * Get the motor speed, usually in meters per second. + * + * @return + */ + /** + * Set the motor speed, usually in meters per second. + * + * @param speed + */ + var motorSpeed: Float + get() = m_motorSpeed + set(speed) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_motorSpeed = speed + } + + /** + * Set the maximum motor force, usually in N. + * + * @param force + */ + var maxMotorForce: Float + get() = m_maxMotorForce + set(force) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_maxMotorForce = force + } + + init { + lowerLimit = def.lowerTranslation + upperLimit = def.upperTranslation + m_maxMotorForce = def.maxMotorForce + m_motorSpeed = def.motorSpeed + isLimitEnabled = def.enableLimit + isMotorEnabled = def.enableMotor + m_limitState = LimitState.INACTIVE + m_K = Mat33() + m_axis = Vec2() + m_perp = Vec2() + m_motorMass = 0.0f + m_motorImpulse = 0.0f + + m_localXAxisA.normalize() + Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA) + m_referenceAngleRadians = def.referenceAngleRadians + + + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(m_localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(m_localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + val temp = pool.popVec2() + temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z) + argOut.set(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt) + pool.pushVec2(1) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return inv_dt * m_impulse.y + } + + /** + * Enable/disable the joint limit. + * + * @param flag + */ + fun enableLimit(flag: Boolean) { + if (flag != isLimitEnabled) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + isLimitEnabled = flag + m_impulse.z = 0.0f + } + } + + /** + * Set the joint limits, usually in meters. + * + * @param lower + * @param upper + */ + fun setLimits(lower: Float, upper: Float) { + assert(lower <= upper) + if (lower != lowerLimit || upper != upperLimit) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + lowerLimit = lower + upperLimit = upper + m_impulse.z = 0.0f + } + } + + /** + * Enable/disable the joint motor. + * + * @param flag + */ + fun enableMotor(flag: Boolean) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + isMotorEnabled = flag + } + + /** + * Get the current motor force, usually in N. + * + * @param inv_dt + * @return + */ + fun getMotorForce(inv_dt: Float): Float { + return m_motorImpulse * inv_dt + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val d = pool.popVec2() + val temp = pool.popVec2() + val rA = pool.popVec2() + val rB = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB) + d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA) + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + // Compute motor Jacobian and effective mass. + run { + Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis) + temp.set(d).addLocal(rA) + m_a1 = Vec2.cross(temp, m_axis) + m_a2 = Vec2.cross(rB, m_axis) + + m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2 + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass + } + } + + // Prismatic constraint. + run { + Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp) + + temp.set(d).addLocal(rA) + m_s1 = Vec2.cross(temp, m_perp) + m_s2 = Vec2.cross(rB, m_perp) + + val k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2 + val k12 = iA * m_s1 + iB * m_s2 + val k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2 + var k22 = iA + iB + if (k22 == 0.0f) { + // For bodies with fixed rotation. + k22 = 1.0f + } + val k23 = iA * m_a1 + iB * m_a2 + val k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2 + + m_K.ex.set(k11, k12, k13) + m_K.ey.set(k12, k22, k23) + m_K.ez.set(k13, k23, k33) + } + + // Compute motor and limit terms. + if (isLimitEnabled) { + + val jointTranslation = Vec2.dot(m_axis, d) + if (MathUtils.abs(upperLimit - lowerLimit) < 2.0f * Settings.linearSlop) { + m_limitState = LimitState.EQUAL + } else if (jointTranslation <= lowerLimit) { + if (m_limitState !== LimitState.AT_LOWER) { + m_limitState = LimitState.AT_LOWER + m_impulse.z = 0.0f + } + } else if (jointTranslation >= upperLimit) { + if (m_limitState !== LimitState.AT_UPPER) { + m_limitState = LimitState.AT_UPPER + m_impulse.z = 0.0f + } + } else { + m_limitState = LimitState.INACTIVE + m_impulse.z = 0.0f + } + } else { + m_limitState = LimitState.INACTIVE + m_impulse.z = 0.0f + } + + if (!isMotorEnabled) { + m_motorImpulse = 0.0f + } + + if (data.step!!.warmStarting) { + // Account for variable time step. + m_impulse.mulLocal(data.step!!.dtRatio) + m_motorImpulse *= data.step!!.dtRatio + + val P = pool.popVec2() + temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z) + P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp) + + val LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1 + val LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2 + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + + pool.pushVec2(1) + } else { + m_impulse.setZero() + m_motorImpulse = 0.0f + } + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushRot(2) + pool.pushVec2(4) + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val temp = pool.popVec2() + + // Solve linear motor constraint. + if (isMotorEnabled && m_limitState !== LimitState.EQUAL) { + temp.set(vB).subLocal(vA) + val Cdot = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA + var impulse = m_motorMass * (m_motorSpeed - Cdot) + val oldImpulse = m_motorImpulse + val maxImpulse = data.step!!.dt * m_maxMotorForce + m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = m_motorImpulse - oldImpulse + + val P = pool.popVec2() + P.set(m_axis).mulLocal(impulse) + val LA = impulse * m_a1 + val LB = impulse * m_a2 + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + + pool.pushVec2(1) + } + + val Cdot1 = pool.popVec2() + temp.set(vB).subLocal(vA) + Cdot1.x = Vec2.dot(m_perp, temp) + m_s2 * wB - m_s1 * wA + Cdot1.y = wB - wA + // System.out.println(Cdot1); + + if (isLimitEnabled && m_limitState !== LimitState.INACTIVE) { + // Solve prismatic and limit constraint in block form. + val Cdot2: Float + temp.set(vB).subLocal(vA) + Cdot2 = Vec2.dot(m_axis, temp) + m_a2 * wB - m_a1 * wA + + val Cdot = pool.popVec3() + Cdot.set(Cdot1.x, Cdot1.y, Cdot2) + + val f1 = pool.popVec3() + val df = pool.popVec3() + + f1.set(m_impulse) + m_K.solve33ToOut(Cdot.negateLocal(), df) + // Cdot.negateLocal(); not used anymore + m_impulse.addLocal(df) + + if (m_limitState === LimitState.AT_LOWER) { + m_impulse.z = MathUtils.max(m_impulse.z, 0.0f) + } else if (m_limitState === LimitState.AT_UPPER) { + m_impulse.z = MathUtils.min(m_impulse.z, 0.0f) + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + + // f1(1:2) + val b = pool.popVec2() + val f2r = pool.popVec2() + + temp.set(m_K.ez.x, m_K.ez.y).mulLocal(m_impulse.z - f1.z) + b.set(Cdot1).negateLocal().subLocal(temp) + + m_K.solve22ToOut(b, f2r) + f2r.addLocal(f1.x, f1.y) + m_impulse.x = f2r.x + m_impulse.y = f2r.y + + df.set(m_impulse).subLocal(f1) + + val P = pool.popVec2() + temp.set(m_axis).mulLocal(df.z) + P.set(m_perp).mulLocal(df.x).addLocal(temp) + + val LA = df.x * m_s1 + df.y + df.z * m_a1 + val LB = df.x * m_s2 + df.y + df.z * m_a2 + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + + pool.pushVec2(3) + pool.pushVec3(3) + } else { + // Limit is inactive, just solve the prismatic constraint in block form. + val df = pool.popVec2() + m_K.solve22ToOut(Cdot1.negateLocal(), df) + Cdot1.negateLocal() + + m_impulse.x += df.x + m_impulse.y += df.y + + val P = pool.popVec2() + P.set(m_perp).mulLocal(df.x) + val LA = df.x * m_s1 + df.y + val LB = df.x * m_s2 + df.y + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + + pool.pushVec2(2) + } + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(2) + } + + + override fun solvePositionConstraints(data: SolverData): Boolean { + + val qA = pool.popRot() + val qB = pool.popRot() + val rA = pool.popVec2() + val rB = pool.popVec2() + val d = pool.popVec2() + val axis = pool.popVec2() + val perp = pool.popVec2() + val temp = pool.popVec2() + val C1 = pool.popVec2() + + val impulse = pool.popVec3() + + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + qA.setRadians(aA) + qB.setRadians(aB) + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + // Compute fresh Jacobians + Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB) + d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + + Rot.mulToOutUnsafe(qA, m_localXAxisA, axis) + val a1 = Vec2.cross(temp.set(d).addLocal(rA), axis) + val a2 = Vec2.cross(rB, axis) + Rot.mulToOutUnsafe(qA, m_localYAxisA, perp) + + val s1 = Vec2.cross(temp.set(d).addLocal(rA), perp) + val s2 = Vec2.cross(rB, perp) + + C1.x = Vec2.dot(perp, d) + C1.y = aB - aA - m_referenceAngleRadians + + var linearError = MathUtils.abs(C1.x) + val angularError = MathUtils.abs(C1.y) + + var active = false + var C2 = 0.0f + if (isLimitEnabled) { + val translation = Vec2.dot(axis, d) + if (MathUtils.abs(upperLimit - lowerLimit) < 2.0f * Settings.linearSlop) { + // Prevent large angular corrections + C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, + Settings.maxLinearCorrection) + linearError = MathUtils.max(linearError, MathUtils.abs(translation)) + active = true + } else if (translation <= lowerLimit) { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.clamp(translation - lowerLimit + Settings.linearSlop, + -Settings.maxLinearCorrection, 0.0f) + linearError = MathUtils.max(linearError, lowerLimit - translation) + active = true + } else if (translation >= upperLimit) { + // Prevent large linear corrections and allow some slop. + C2 = MathUtils.clamp(translation - upperLimit - Settings.linearSlop, 0.0f, + Settings.maxLinearCorrection) + linearError = MathUtils.max(linearError, translation - upperLimit) + active = true + } + } + + if (active) { + val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2 + val k12 = iA * s1 + iB * s2 + val k13 = iA * s1 * a1 + iB * s2 * a2 + var k22 = iA + iB + if (k22 == 0.0f) { + // For fixed rotation + k22 = 1.0f + } + val k23 = iA * a1 + iB * a2 + val k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2 + + val K = pool.popMat33() + K.ex.set(k11, k12, k13) + K.ey.set(k12, k22, k23) + K.ez.set(k13, k23, k33) + + val C = pool.popVec3() + C.x = C1.x + C.y = C1.y + C.z = C2 + + K.solve33ToOut(C.negateLocal(), impulse) + pool.pushVec3(1) + pool.pushMat33(1) + } else { + val k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2 + val k12 = iA * s1 + iB * s2 + var k22 = iA + iB + if (k22 == 0.0f) { + k22 = 1.0f + } + + val K = pool.popMat22() + K.ex.set(k11, k12) + K.ey.set(k12, k22) + + // temp is impulse1 + K.solveToOut(C1.negateLocal(), temp) + C1.negateLocal() + + impulse.x = temp.x + impulse.y = temp.y + impulse.z = 0.0f + + pool.pushMat22(1) + } + + val Px = impulse.x * perp.x + impulse.z * axis.x + val Py = impulse.x * perp.y + impulse.z * axis.y + val LA = impulse.x * s1 + impulse.y + impulse.z * a1 + val LB = impulse.x * s2 + impulse.y + impulse.z * a2 + + cA.x -= mA * Px + cA.y -= mA * Py + aA -= iA * LA + cB.x += mB * Px + cB.y += mB * Py + aB += iB * LB + + // data.positions[m_indexA].c.set(cA); + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c.set(cB); + data.positions!![m_indexB].a = aB + + pool.pushVec2(7) + pool.pushVec3(1) + pool.pushRot(2) + + return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJointDef.kt new file mode 100644 index 0000000..2e4deee --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PrismaticJointDef.kt @@ -0,0 +1,130 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Prismatic joint definition. This requires defining a line of motion using an axis and an anchor + * point. The definition uses local anchor points and a local axis so that the initial configuration + * can violate the constraint slightly. The joint translation is zero when the local anchor points + * coincide in world space. Using local anchors and a local axis helps when saving and loading a + * game. + * + * @warning at least one body should by dynamic with a non-fixed rotation. + * @author Daniel + */ +class PrismaticJointDef : JointDef(JointType.PRISMATIC) { + + + /** + * The local anchor point relative to body1's origin. + */ + + val localAnchorA: Vec2 = Vec2() + + /** + * The local anchor point relative to body2's origin. + */ + + val localAnchorB: Vec2 = Vec2() + + /** + * The local translation axis in body1. + */ + + val localAxisA: Vec2 = Vec2(1.0f, 0.0f) + + /** + * The constrained angle in radians between the bodies: body2_angle - body1_angle. + */ + var referenceAngleRadians: Float = 0f + + /** + * The constrained angle in degrees between the bodies: body2_angle - body1_angle. + */ + var referenceAngleDegrees: Float + set(value) { referenceAngleRadians = value * MathUtils.DEG2RAD } + get() = referenceAngleRadians * MathUtils.RAD2DEG + + /** + * The constrained angle between the bodies: body2_angle - body1_angle. + */ + var referenceAngle: Angle + set(value) { referenceAngleRadians = value.radians.toFloat() } + get() = referenceAngleRadians.radians + + /** + * Enable/disable the joint limit. + */ + + var enableLimit: Boolean = false + + /** + * The lower translation limit, usually in meters. + */ + + var lowerTranslation: Float = 0f + + /** + * The upper translation limit, usually in meters. + */ + + var upperTranslation: Float = 0f + + /** + * Enable/disable the joint motor. + */ + + var enableMotor: Boolean = false + + /** + * The maximum motor torque, usually in N-m. + */ + + var maxMotorForce: Float = 0f + + /** + * The desired motor speed in radians per second. + */ + + var motorSpeed: Float = 0f + + /** + * Initialize the bodies, anchors, axis, and reference angle using the world anchor and world + * axis. + */ + fun initialize(b1: Body, b2: Body, anchor: Vec2, axis: Vec2) { + bodyA = b1 + bodyB = b2 + bodyA!!.getLocalPointToOut(anchor, localAnchorA) + bodyB!!.getLocalPointToOut(anchor, localAnchorB) + bodyA!!.getLocalVectorToOut(axis, localAxisA) + referenceAngleRadians = bodyB!!.angleRadians - bodyA!!.angleRadians + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJoint.kt new file mode 100644 index 0000000..7da3be1 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJoint.kt @@ -0,0 +1,373 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 12:12:02 PM Jan 23, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +/** + * The pulley joint is connected to two bodies and two fixed ground points. The pulley supports a + * ratio such that: length1 + ratio * length2 <= constant Yes, the force transmitted is scaled by + * the ratio. Warning: the pulley joint can get a bit squirrelly by itself. They often work better + * when combined with prismatic joints. You should also cover the the anchor points with static + * shapes to prevent one side from going to zero length. + * + * @author Daniel Murphy + */ +class PulleyJoint constructor(argWorldPool: IWorldPool, def: PulleyJointDef) : Joint(argWorldPool, def) { + + val _groundAnchorA = Vec2() + + val _groundAnchorB = Vec2() + + fun getGroundAnchorA() = _groundAnchorA + fun getGroundAnchorB() = _groundAnchorB + + + val lengthA: Float + + val lengthB: Float + + // Solver shared + + val localAnchorA = Vec2() + + val localAnchorB = Vec2() + private val m_constant: Float + val ratio: Float + private var m_impulse: Float = 0.toFloat() + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_uA = Vec2() + private val m_uB = Vec2() + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private var m_mass: Float = 0.toFloat() + + val currentLengthA: Float + get() { + val p = pool.popVec2() + bodyA!!.getWorldPointToOut(localAnchorA, p) + p.subLocal(_groundAnchorA) + val length = p.length() + pool.pushVec2(1) + return length + } + + val currentLengthB: Float + get() { + val p = pool.popVec2() + bodyB!!.getWorldPointToOut(localAnchorB, p) + p.subLocal(_groundAnchorB) + val length = p.length() + pool.pushVec2(1) + return length + } + + val length1: Float + get() { + val p = pool.popVec2() + bodyA!!.getWorldPointToOut(localAnchorA, p) + p.subLocal(_groundAnchorA) + + val len = p.length() + pool.pushVec2(1) + return len + } + + val length2: Float + get() { + val p = pool.popVec2() + bodyB!!.getWorldPointToOut(localAnchorB, p) + p.subLocal(_groundAnchorB) + + val len = p.length() + pool.pushVec2(1) + return len + } + + init { + _groundAnchorA.set(def.groundAnchorA) + _groundAnchorB.set(def.groundAnchorB) + localAnchorA.set(def.localAnchorA) + localAnchorB.set(def.localAnchorB) + + assert(def.ratio != 0.0f) + ratio = def.ratio + + lengthA = def.lengthA + lengthB = def.lengthB + + m_constant = def.lengthA + ratio * def.lengthB + m_impulse = 0.0f + } + + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_uB).mulLocal(m_impulse).mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return 0f + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), m_rB) + + m_uA.set(cA).addLocal(m_rA).subLocal(_groundAnchorA) + m_uB.set(cB).addLocal(m_rB).subLocal(_groundAnchorB) + + val lengthA = m_uA.length() + val lengthB = m_uB.length() + + if (lengthA > 10f * Settings.linearSlop) { + m_uA.mulLocal(1.0f / lengthA) + } else { + m_uA.setZero() + } + + if (lengthB > 10f * Settings.linearSlop) { + m_uB.mulLocal(1.0f / lengthB) + } else { + m_uB.setZero() + } + + // Compute effective mass. + val ruA = Vec2.cross(m_rA, m_uA) + val ruB = Vec2.cross(m_rB, m_uB) + + val mA = m_invMassA + m_invIA * ruA * ruA + val mB = m_invMassB + m_invIB * ruB * ruB + + m_mass = mA + ratio * ratio * mB + + if (m_mass > 0.0f) { + m_mass = 1.0f / m_mass + } + + if (data.step!!.warmStarting) { + + // Scale impulses to support variable time steps. + m_impulse *= data.step!!.dtRatio + + // Warm starting. + val PA = pool.popVec2() + val PB = pool.popVec2() + + PA.set(m_uA).mulLocal(-m_impulse) + PB.set(m_uB).mulLocal(-ratio * m_impulse) + + vA.x += m_invMassA * PA.x + vA.y += m_invMassA * PA.y + wA += m_invIA * Vec2.cross(m_rA, PA) + vB.x += m_invMassB * PB.x + vB.y += m_invMassB * PB.y + wB += m_invIB * Vec2.cross(m_rB, PB) + + pool.pushVec2(2) + } else { + m_impulse = 0.0f + } + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(1) + pool.pushRot(2) + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val vpA = pool.popVec2() + val vpB = pool.popVec2() + val PA = pool.popVec2() + val PB = pool.popVec2() + + Vec2.crossToOutUnsafe(wA, m_rA, vpA) + vpA.addLocal(vA) + Vec2.crossToOutUnsafe(wB, m_rB, vpB) + vpB.addLocal(vB) + + val Cdot = -Vec2.dot(m_uA, vpA) - ratio * Vec2.dot(m_uB, vpB) + val impulse = -m_mass * Cdot + m_impulse += impulse + + PA.set(m_uA).mulLocal(-impulse) + PB.set(m_uB).mulLocal(-ratio * impulse) + vA.x += m_invMassA * PA.x + vA.y += m_invMassA * PA.y + wA += m_invIA * Vec2.cross(m_rA, PA) + vB.x += m_invMassB * PB.x + vB.y += m_invMassB * PB.y + wB += m_invIB * Vec2.cross(m_rB, PB) + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(4) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val qA = pool.popRot() + val qB = pool.popRot() + val rA = pool.popVec2() + val rB = pool.popVec2() + val uA = pool.popVec2() + val uB = pool.popVec2() + val temp = pool.popVec2() + val PA = pool.popVec2() + val PB = pool.popVec2() + + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + qA.setRadians(aA) + qB.setRadians(aB) + + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), rB) + + uA.set(cA).addLocal(rA).subLocal(_groundAnchorA) + uB.set(cB).addLocal(rB).subLocal(_groundAnchorB) + + val lengthA = uA.length() + val lengthB = uB.length() + + if (lengthA > 10.0f * Settings.linearSlop) { + uA.mulLocal(1.0f / lengthA) + } else { + uA.setZero() + } + + if (lengthB > 10.0f * Settings.linearSlop) { + uB.mulLocal(1.0f / lengthB) + } else { + uB.setZero() + } + + // Compute effective mass. + val ruA = Vec2.cross(rA, uA) + val ruB = Vec2.cross(rB, uB) + + val mA = m_invMassA + m_invIA * ruA * ruA + val mB = m_invMassB + m_invIB * ruB * ruB + + var mass = mA + ratio * ratio * mB + + if (mass > 0.0f) { + mass = 1.0f / mass + } + + val C = m_constant - lengthA - ratio * lengthB + val linearError = MathUtils.abs(C) + + val impulse = -mass * C + + PA.set(uA).mulLocal(-impulse) + PB.set(uB).mulLocal(-ratio * impulse) + + cA.x += m_invMassA * PA.x + cA.y += m_invMassA * PA.y + aA += m_invIA * Vec2.cross(rA, PA) + cB.x += m_invMassB * PB.x + cB.y += m_invMassB * PB.y + aB += m_invIB * Vec2.cross(rB, PB) + + // data.positions[m_indexA].c.set(cA); + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c.set(cB); + data.positions!![m_indexB].a = aB + + pool.pushRot(2) + pool.pushVec2(7) + + return linearError < Settings.linearSlop + } + + companion object { + + val MIN_PULLEY_LENGTH = 2.0f + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJointDef.kt new file mode 100644 index 0000000..ed73f9b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/PulleyJointDef.kt @@ -0,0 +1,105 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 12:11:41 PM Jan 23, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body +import org.jbox2d.internal.assert + +/** + * Pulley joint definition. This requires two ground anchors, two dynamic body anchor points, and a + * pulley ratio. + * + * @author Daniel Murphy + */ +class PulleyJointDef : JointDef(JointType.PULLEY) { + + /** + * The first ground anchor in world coordinates. This point never moves. + */ + + var groundAnchorA: Vec2 = Vec2(-1.0f, 1.0f) + + /** + * The second ground anchor in world coordinates. This point never moves. + */ + + var groundAnchorB: Vec2 = Vec2(1.0f, 1.0f) + + /** + * The local anchor point relative to bodyA's origin. + */ + + var localAnchorA: Vec2 = Vec2(-1.0f, 0.0f) + + /** + * The local anchor point relative to bodyB's origin. + */ + + var localAnchorB: Vec2 = Vec2(1.0f, 0.0f) + + /** + * The a reference length for the segment attached to bodyA. + */ + + var lengthA: Float = 0f + + /** + * The a reference length for the segment attached to bodyB. + */ + + var lengthB: Float = 0f + + /** + * The pulley ratio, used to simulate a block-and-tackle. + */ + + var ratio: Float = 1f + + init { + collideConnected = true + } + + /** + * Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. + */ + fun initialize(b1: Body, b2: Body, ga1: Vec2, ga2: Vec2, anchor1: Vec2, anchor2: Vec2, r: Float) { + bodyA = b1 + bodyB = b2 + groundAnchorA = ga1 + groundAnchorB = ga2 + localAnchorA = bodyA!!.getLocalPoint(anchor1) + localAnchorB = bodyB!!.getLocalPoint(anchor2) + val d1 = anchor1.sub(ga1) + lengthA = d1.length() + val d2 = anchor2.sub(ga2) + lengthB = d2.length() + ratio = r + assert(ratio > Settings.EPSILON) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJoint.kt new file mode 100644 index 0000000..595ef1e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJoint.kt @@ -0,0 +1,539 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.Mat33 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IWorldPool + +//Point-to-point constraint +//C = p2 - p1 +//Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +//J = [-I -r1_skew I r2_skew ] +//Identity used: +//w k % (rx i + ry j) = w * (-ry i + rx j) + +//Motor constraint +//Cdot = w2 - w1 +//J = [0 0 -1 0 0 1] +//K = invI1 + invI2 + +/** + * A revolute joint constrains two bodies to share a common point while they are free to rotate + * about the point. The relative rotation about the shared point is the joint angle. You can limit + * the relative rotation with a joint limit that specifies a lower and upper angle. You can use a + * motor to drive the relative rotation about the shared point. A maximum motor torque is provided + * so that infinite forces are not generated. + * + * @author Daniel Murphy + */ +class RevoluteJoint(argWorld: IWorldPool, def: RevoluteJointDef) : Joint(argWorld, def) { + + // Solver shared + + val m_localAnchorA = Vec2() + + val m_localAnchorB = Vec2() + private val m_impulse = Vec3() + private var m_motorImpulse: Float = 0.toFloat() + + var isMotorEnabled: Boolean = false + private set + private var m_maxMotorTorque: Float = 0.toFloat() + private var m_motorSpeed: Float = 0.toFloat() + + var isLimitEnabled: Boolean = false + private set + var m_referenceAngleRadians: Float = 0.toFloat() + protected set + var m_referenceAngleDegrees: Float + protected set(value) { m_referenceAngleRadians = value * MathUtils.DEG2RAD } + get() = m_referenceAngleRadians * MathUtils.RAD2DEG + var m_referenceAngle: Angle + protected set(value) { m_referenceAngleRadians = value.radians.toFloat() } + get() = m_referenceAngleRadians.radians + var lowerLimit: Float = 0.toFloat() + private set + var upperLimit: Float = 0.toFloat() + private set + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_mass = Mat33() // effective mass for point-to-point constraint. + private var m_motorMass: Float = 0.toFloat() // effective mass for motor/limit angular constraint. + private var m_limitState: LimitState? = null + + val jointAngleRadians: Float + get() { + val b1 = bodyA + val b2 = bodyB + return b2!!.sweep.a - b1!!.sweep.a - m_referenceAngleRadians + } + + val jointAngleDegrees: Float get() = jointAngleRadians * MathUtils.RAD2DEG + + val jointAngle: Angle get() = jointAngleRadians.radians + + val jointSpeed: Float + get() { + val b1 = bodyA + val b2 = bodyB + return b2!!._angularVelocity - b1!!._angularVelocity + } + + var motorSpeed: Float + get() = m_motorSpeed + set(speed) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_motorSpeed = speed + } + + var maxMotorTorque: Float + get() = m_maxMotorTorque + set(torque) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_maxMotorTorque = torque + } + + init { + m_localAnchorA.set(def.localAnchorA) + m_localAnchorB.set(def.localAnchorB) + m_referenceAngleRadians = def.referenceAngleRadians + + m_motorImpulse = 0f + + lowerLimit = def.lowerAngleRadians + upperLimit = def.upperAngleRadians + m_maxMotorTorque = def.maxMotorTorque + m_motorSpeed = def.motorSpeed + isLimitEnabled = def.enableLimit + isMotorEnabled = def.enableMotor + m_limitState = LimitState.INACTIVE + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + // Vec2 cA = data.positions[m_indexA].c; + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + // Vec2 cB = data.positions[m_indexB].c; + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB) + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val fixedRotation = iA + iB == 0.0f + + m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB + m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB + m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB + m_mass.ex.y = m_mass.ey.x + m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB + m_mass.ez.y = m_rA.x * iA + m_rB.x * iB + m_mass.ex.z = m_mass.ez.x + m_mass.ey.z = m_mass.ez.y + m_mass.ez.z = iA + iB + + m_motorMass = iA + iB + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass + } + + if (!isMotorEnabled || fixedRotation) { + m_motorImpulse = 0.0f + } + + if (isLimitEnabled && !fixedRotation) { + val jointAngle = aB - aA - m_referenceAngleRadians + if (MathUtils.abs(upperLimit - lowerLimit) < 2.0f * Settings.angularSlop) { + m_limitState = LimitState.EQUAL + } else if (jointAngle <= lowerLimit) { + if (m_limitState !== LimitState.AT_LOWER) { + m_impulse.z = 0.0f + } + m_limitState = LimitState.AT_LOWER + } else if (jointAngle >= upperLimit) { + if (m_limitState !== LimitState.AT_UPPER) { + m_impulse.z = 0.0f + } + m_limitState = LimitState.AT_UPPER + } else { + m_limitState = LimitState.INACTIVE + m_impulse.z = 0.0f + } + } else { + m_limitState = LimitState.INACTIVE + } + + if (data.step!!.warmStarting) { + val P = pool.popVec2() + // Scale impulses to support a variable time step. + m_impulse.x *= data.step!!.dtRatio + m_impulse.y *= data.step!!.dtRatio + m_motorImpulse *= data.step!!.dtRatio + + P.x = m_impulse.x + P.y = m_impulse.y + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * (Vec2.cross(m_rA, P) + m_motorImpulse + m_impulse.z) + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * (Vec2.cross(m_rB, P) + m_motorImpulse + m_impulse.z) + pool.pushVec2(1) + } else { + m_impulse.setZero() + m_motorImpulse = 0.0f + } + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(1) + pool.pushRot(2) + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val fixedRotation = iA + iB == 0.0f + + // Solve motor constraint. + if (isMotorEnabled && m_limitState !== LimitState.EQUAL && !fixedRotation) { + val Cdot = wB - wA - m_motorSpeed + var impulse = -m_motorMass * Cdot + val oldImpulse = m_motorImpulse + val maxImpulse = data.step!!.dt * m_maxMotorTorque + m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = m_motorImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + val temp = pool.popVec2() + + // Solve limit constraint. + if (isLimitEnabled && m_limitState !== LimitState.INACTIVE && !fixedRotation) { + + val Cdot1 = pool.popVec2() + val Cdot = pool.popVec3() + + // Solve point-to-point constraint + Vec2.crossToOutUnsafe(wA, m_rA, temp) + Vec2.crossToOutUnsafe(wB, m_rB, Cdot1) + Cdot1.addLocal(vB).subLocal(vA).subLocal(temp) + val Cdot2 = wB - wA + Cdot.set(Cdot1.x, Cdot1.y, Cdot2) + + val impulse = pool.popVec3() + m_mass.solve33ToOut(Cdot, impulse) + impulse.negateLocal() + + if (m_limitState === LimitState.EQUAL) { + m_impulse.addLocal(impulse) + } else if (m_limitState === LimitState.AT_LOWER) { + val newImpulse = m_impulse.z + impulse.z + if (newImpulse < 0.0f) { + val rhs = pool.popVec2() + rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1) + m_mass.solve22ToOut(rhs, temp) + impulse.x = temp.x + impulse.y = temp.y + impulse.z = -m_impulse.z + m_impulse.x += temp.x + m_impulse.y += temp.y + m_impulse.z = 0.0f + pool.pushVec2(1) + } else { + m_impulse.addLocal(impulse) + } + } else if (m_limitState === LimitState.AT_UPPER) { + val newImpulse = m_impulse.z + impulse.z + if (newImpulse > 0.0f) { + val rhs = pool.popVec2() + rhs.set(m_mass.ez.x, m_mass.ez.y).mulLocal(m_impulse.z).subLocal(Cdot1) + m_mass.solve22ToOut(rhs, temp) + impulse.x = temp.x + impulse.y = temp.y + impulse.z = -m_impulse.z + m_impulse.x += temp.x + m_impulse.y += temp.y + m_impulse.z = 0.0f + pool.pushVec2(1) + } else { + m_impulse.addLocal(impulse) + } + } + val P = pool.popVec2() + + P.set(impulse.x, impulse.y) + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * (Vec2.cross(m_rA, P) + impulse.z) + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * (Vec2.cross(m_rB, P) + impulse.z) + + pool.pushVec2(2) + pool.pushVec3(2) + } else { + + // Solve point-to-point constraint + val Cdot = pool.popVec2() + val impulse = pool.popVec2() + + Vec2.crossToOutUnsafe(wA, m_rA, temp) + Vec2.crossToOutUnsafe(wB, m_rB, Cdot) + Cdot.addLocal(vB).subLocal(vA).subLocal(temp) + m_mass.solve22ToOut(Cdot.negateLocal(), impulse) // just leave negated + + m_impulse.x += impulse.x + m_impulse.y += impulse.y + + vA.x -= mA * impulse.x + vA.y -= mA * impulse.y + wA -= iA * Vec2.cross(m_rA, impulse) + + vB.x += mB * impulse.x + vB.y += mB * impulse.y + wB += iB * Vec2.cross(m_rB, impulse) + + pool.pushVec2(2) + } + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(1) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val qA = pool.popRot() + val qB = pool.popRot() + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + qA.setRadians(aA) + qB.setRadians(aB) + + var angularError = 0.0f + var positionError = 0.0f + + val fixedRotation = m_invIA + m_invIB == 0.0f + + // Solve angular limit constraint. + if (isLimitEnabled && m_limitState !== LimitState.INACTIVE && !fixedRotation) { + val angle = aB - aA - m_referenceAngleRadians + var limitImpulse = 0.0f + + if (m_limitState === LimitState.EQUAL) { + // Prevent large angular corrections + val C = MathUtils.clamp(angle - lowerLimit, -Settings.maxAngularCorrection, + Settings.maxAngularCorrection) + limitImpulse = -m_motorMass * C + angularError = MathUtils.abs(C) + } else if (m_limitState === LimitState.AT_LOWER) { + var C = angle - lowerLimit + angularError = -C + + // Prevent large angular corrections and allow some slop. + C = MathUtils.clamp(C + Settings.angularSlop, -Settings.maxAngularCorrection, 0.0f) + limitImpulse = -m_motorMass * C + } else if (m_limitState === LimitState.AT_UPPER) { + var C = angle - upperLimit + angularError = C + + // Prevent large angular corrections and allow some slop. + C = MathUtils.clamp(C - Settings.angularSlop, 0.0f, Settings.maxAngularCorrection) + limitImpulse = -m_motorMass * C + } + + aA -= m_invIA * limitImpulse + aB += m_invIB * limitImpulse + } + // Solve point-to-point constraint. + run { + qA.setRadians(aA) + qB.setRadians(aB) + + val rA = pool.popVec2() + val rB = pool.popVec2() + val C = pool.popVec2() + val impulse = pool.popVec2() + + Rot.mulToOutUnsafe(qA, C.set(m_localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, C.set(m_localAnchorB).subLocal(m_localCenterB), rB) + C.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + positionError = C.length() + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val K = pool.popMat22() + K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y + K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y + K.ey.x = K.ex.y + K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x + K.solveToOut(C, impulse) + impulse.negateLocal() + + cA.x -= mA * impulse.x + cA.y -= mA * impulse.y + aA -= iA * Vec2.cross(rA, impulse) + + cB.x += mB * impulse.x + cB.y += mB * impulse.y + aB += iB * Vec2.cross(rB, impulse) + + pool.pushVec2(4) + pool.pushMat22(1) + } + // data.positions[m_indexA].c.set(cA); + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c.set(cB); + data.positions!![m_indexB].a = aB + + pool.pushRot(2) + + return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(m_localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(m_localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_impulse.x, m_impulse.y).mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return inv_dt * m_impulse.z + } + + fun enableMotor(flag: Boolean) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + isMotorEnabled = flag + } + + fun getMotorTorque(inv_dt: Float): Float { + return m_motorImpulse * inv_dt + } + + fun enableLimit(flag: Boolean) { + if (flag != isLimitEnabled) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + isLimitEnabled = flag + m_impulse.z = 0.0f + } + } + + fun setLimits(lower: Float, upper: Float) { + assert(lower <= upper) + if (lower != lowerLimit || upper != upperLimit) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_impulse.z = 0.0f + lowerLimit = lower + upperLimit = upper + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJointDef.kt new file mode 100644 index 0000000..dc8e0c4 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RevoluteJointDef.kt @@ -0,0 +1,163 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/* + * JBox2D - A Java Port of Erin Catto's Box2D + * + * JBox2D homepage: http://jbox2d.sourceforge.net/ + * Box2D homepage: http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Revolute joint definition. This requires defining an anchor point where the bodies are joined. + * The definition uses local anchor points so that the initial configuration can violate the + * constraint slightly. You also need to specify the initial relative angle for joint limits. This + * helps when saving and loading a game. The local anchor points are measured from the body's origin + * rather than the center of mass because:

+ * + * * you might not know where the center of mass will be. + * * if you add/remove shapes from a body and recompute the mass, the joints will be broken. + * + */ +class RevoluteJointDef : JointDef(JointType.REVOLUTE) { + + /** + * The local anchor point relative to body1's origin. + */ + + var localAnchorA: Vec2 = Vec2(0.0f, 0.0f) + + /** + * The local anchor point relative to body2's origin. + */ + + var localAnchorB: Vec2 = Vec2(0.0f, 0.0f) + + /** + * The body2 angle minus body1 angle in the reference state (radians). + */ + var referenceAngleRadians: Float = 0f + + /** + * The body2 angle minus body1 angle in the reference state (degrees). + */ + var referenceAngleDegrees: Float + set(value) { referenceAngleRadians = value * MathUtils.DEG2RAD } + get() = referenceAngleRadians * MathUtils.RAD2DEG + + /** + * The body2 angle minus body1 angle in the reference state. + */ + var referenceAngle: Angle + set(value) { referenceAngleRadians = value.radians.toFloat() } + get() = referenceAngleRadians.radians + + /** + * A flag to enable joint limits. + */ + + var enableLimit: Boolean = false + + /** The lower angle for the joint limit (radians). */ + var lowerAngleRadians: Float = 0f + + /** The lower angle for the joint limit (degrees). */ + var lowerAngleDegrees: Float + set(value) { lowerAngleRadians = value * MathUtils.DEG2RAD } + get() = lowerAngleRadians * MathUtils.RAD2DEG + + /** The lower angle for the joint limit. */ + var lowerAngle: Angle + set(value) { lowerAngleRadians = value.radians.toFloat() } + get() = lowerAngleRadians.radians + + /** The upper angle for the joint limit (radians). */ + var upperAngleRadians: Float = 0f + + /** The upper angle for the joint limit (degrees). */ + var upperAngleDegrees: Float + set(value) { upperAngleRadians = value * MathUtils.DEG2RAD } + get() = upperAngleRadians * MathUtils.RAD2DEG + + /** The lower angle for the joint limit. */ + var upperAngle: Angle + set(value) { upperAngleRadians = value.radians.toFloat() } + get() = upperAngleRadians.radians + + /** + * A flag to enable the joint motor. + */ + + var enableMotor: Boolean = false + + /** + * The desired motor speed. Usually in radians per second. + */ + + var motorSpeed: Float = 0f + + /** + * The maximum motor torque used to achieve the desired motor speed. Usually in N-m. + */ + + var maxMotorTorque: Float = 0f + + /** + * Initialize the bodies, anchors, and reference angle using the world anchor. + * + * @param b1 + * @param b2 + * @param anchor + */ + fun initialize(b1: Body, b2: Body, anchor: Vec2) { + bodyA = b1 + bodyB = b2 + bodyA!!.getLocalPointToOut(anchor, localAnchorA) + bodyB!!.getLocalPointToOut(anchor, localAnchorB) + referenceAngleRadians = bodyB!!.angleRadians - bodyA!!.angleRadians + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJoint.kt new file mode 100644 index 0000000..639bc4b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJoint.kt @@ -0,0 +1,254 @@ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.pooling.IWorldPool + +/** + * A rope joint enforces a maximum distance between two points on two bodies. It has no other + * effect. Warning: if you attempt to change the maximum length during the simulation you will get + * some non-physical behavior. A model that would allow you to dynamically modify the length would + * have some sponginess, so I chose not to implement it that way. See DistanceJoint if you want to + * dynamically control length. + * + * @author Daniel Murphy + */ +class RopeJoint(worldPool: IWorldPool, def: RopeJointDef) : Joint(worldPool, def) { + // Solver shared + + val localAnchorA = Vec2() + + val localAnchorB = Vec2() + + var maxLength: Float = 0.toFloat() + private var m_length: Float = 0.toFloat() + private var m_impulse: Float = 0.toFloat() + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_u = Vec2() + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private var m_mass: Float = 0.toFloat() + var limitState: LimitState? = null + private set + + init { + localAnchorA.set(def.localAnchorA) + localAnchorB.set(def.localAnchorB) + + maxLength = def.maxLength + + m_mass = 0.0f + m_impulse = 0.0f + limitState = LimitState.INACTIVE + m_length = 0.0f + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), m_rB) + + m_u.set(cB).addLocal(m_rB).subLocal(cA).subLocal(m_rA) + + m_length = m_u.length() + + val C = m_length - maxLength + if (C > 0.0f) { + limitState = LimitState.AT_UPPER + } else { + limitState = LimitState.INACTIVE + } + + if (m_length > Settings.linearSlop) { + m_u.mulLocal(1.0f / m_length) + } else { + m_u.setZero() + m_mass = 0.0f + m_impulse = 0.0f + pool.pushRot(2) + pool.pushVec2(1) + return + } + + // Compute effective mass. + val crA = Vec2.cross(m_rA, m_u) + val crB = Vec2.cross(m_rB, m_u) + val invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB + + m_mass = if (invMass != 0.0f) 1.0f / invMass else 0.0f + + if (data.step!!.warmStarting) { + // Scale the impulse to support a variable time step. + m_impulse *= data.step!!.dtRatio + + val Px = m_impulse * m_u.x + val Py = m_impulse * m_u.y + vA.x -= m_invMassA * Px + vA.y -= m_invMassA * Py + wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px) + + vB.x += m_invMassB * Px + vB.y += m_invMassB * Py + wB += m_invIB * (m_rB.x * Py - m_rB.y * Px) + } else { + m_impulse = 0.0f + } + + pool.pushRot(2) + pool.pushVec2(1) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + // Cdot = dot(u, v + cross(w, r)) + val vpA = pool.popVec2() + val vpB = pool.popVec2() + val temp = pool.popVec2() + + Vec2.crossToOutUnsafe(wA, m_rA, vpA) + vpA.addLocal(vA) + Vec2.crossToOutUnsafe(wB, m_rB, vpB) + vpB.addLocal(vB) + + val C = m_length - maxLength + var Cdot = Vec2.dot(m_u, temp.set(vpB).subLocal(vpA)) + + // Predictive constraint. + if (C < 0.0f) { + Cdot += data.step!!.inv_dt * C + } + + var impulse = -m_mass * Cdot + val oldImpulse = m_impulse + m_impulse = MathUtils.min(0.0f, m_impulse + impulse) + impulse = m_impulse - oldImpulse + + val Px = impulse * m_u.x + val Py = impulse * m_u.y + vA.x -= m_invMassA * Px + vA.y -= m_invMassA * Py + wA -= m_invIA * (m_rA.x * Py - m_rA.y * Px) + vB.x += m_invMassB * Px + vB.y += m_invMassB * Py + wB += m_invIB * (m_rB.x * Py - m_rB.y * Px) + + pool.pushVec2(3) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + val qA = pool.popRot() + val qB = pool.popRot() + val u = pool.popVec2() + val rA = pool.popVec2() + val rB = pool.popVec2() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), rB) + u.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + + val length = u.normalize() + var C = length - maxLength + + C = MathUtils.clamp(C, 0.0f, Settings.maxLinearCorrection) + + val impulse = -m_mass * C + val Px = impulse * u.x + val Py = impulse * u.y + + cA.x -= m_invMassA * Px + cA.y -= m_invMassA * Py + aA -= m_invIA * (rA.x * Py - rA.y * Px) + cB.x += m_invMassB * Px + cB.y += m_invMassB * Py + aB += m_invIB * (rB.x * Py - rB.y * Px) + + pool.pushRot(2) + pool.pushVec2(4) + + // data.positions[m_indexA].c = cA; + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c = cB; + data.positions!![m_indexB].a = aB + + return length - maxLength < Settings.linearSlop + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_u).mulLocal(inv_dt).mulLocal(m_impulse) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return 0f + } + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJointDef.kt new file mode 100644 index 0000000..07124a4 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/RopeJointDef.kt @@ -0,0 +1,31 @@ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 + +/** + * Rope joint definition. This requires two body anchor points and a maximum lengths. Note: by + * default the connected objects will not collide. see collideConnected in b2JointDef. + * + * @author Daniel Murphy + */ +class RopeJointDef : JointDef(JointType.ROPE) { + + /** + * The local anchor point relative to bodyA's origin. + */ + + val localAnchorA = Vec2(-1f, 0f) + + /** + * The local anchor point relative to bodyB's origin. + */ + + val localAnchorB = Vec2(1f, 0f) + + /** + * The maximum length of the rope. Warning: this must be larger than b2_linearSlop or the joint + * will have no effect. + */ + + var maxLength: Float = 0f +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJoint.kt new file mode 100644 index 0000000..7bf90a2 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJoint.kt @@ -0,0 +1,392 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 3:38:38 AM Jan 15, 2011 + */ +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.Mat33 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.pooling.IWorldPool + +//Point-to-point constraint +//C = p2 - p1 +//Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +//J = [-I -r1_skew I r2_skew ] +//Identity used: +//w k % (rx i + ry j) = w * (-ry i + rx j) + +//Angle constraint +//C = angle2 - angle1 - referenceAngle +//Cdot = w2 - w1 +//J = [0 0 -1 0 0 1] +//K = invI1 + invI2 + +/** + * A weld joint essentially glues two bodies together. A weld joint may distort somewhat because the + * island constraint solver is approximate. + * + * @author Daniel Murphy + */ +class WeldJoint(argWorld: IWorldPool, def: WeldJointDef) : Joint(argWorld, def) { + + + var frequency: Float = def.frequencyHz + + var dampingRatio: Float = def.dampingRatio + private var m_bias: Float = 0.toFloat() + + // Solver shared + + val localAnchorA: Vec2 = Vec2(def.localAnchorA) + + val localAnchorB: Vec2 = Vec2(def.localAnchorB) + val referenceAngleRadians: Float = def.referenceAngleRadians + val referenceAngleDegrees: Float get() = referenceAngleRadians * MathUtils.RAD2DEG + val referenceAngle: Angle get() = referenceAngleRadians.radians + private var m_gamma: Float = 0.toFloat() + private val m_impulse: Vec3 = Vec3(0f, 0f, 0f) + + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_rA = Vec2() + private val m_rB = Vec2() + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + private val m_mass = Mat33() + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + argOut.set(m_impulse.x, m_impulse.y) + argOut.mulLocal(inv_dt) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return inv_dt * m_impulse.z + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + // Vec2 cA = data.positions[m_indexA].c; + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + // Vec2 cB = data.positions[m_indexB].c; + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), m_rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), m_rB) + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] + // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val K = pool.popMat33() + + K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB + K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB + K.ez.x = -m_rA.y * iA - m_rB.y * iB + K.ex.y = K.ey.x + K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB + K.ez.y = m_rA.x * iA + m_rB.x * iB + K.ex.z = K.ez.x + K.ey.z = K.ez.y + K.ez.z = iA + iB + + if (frequency > 0.0f) { + K.getInverse22(m_mass) + + var invM = iA + iB + val m = if (invM > 0.0f) 1.0f / invM else 0.0f + + val C = aB - aA - referenceAngleRadians + + // Frequency + val omega = 2.0f * MathUtils.PI * frequency + + // Damping coefficient + val d = 2.0f * m * dampingRatio * omega + + // Spring stiffness + val k = m * omega * omega + + // magic formulas + val h = data.step!!.dt + m_gamma = h * (d + h * k) + m_gamma = if (m_gamma != 0.0f) 1.0f / m_gamma else 0.0f + m_bias = C * h * k * m_gamma + + invM += m_gamma + m_mass.ez.z = if (invM != 0.0f) 1.0f / invM else 0.0f + } else { + K.getSymInverse33(m_mass) + m_gamma = 0.0f + m_bias = 0.0f + } + + if (data.step!!.warmStarting) { + val P = pool.popVec2() + // Scale impulses to support a variable time step. + m_impulse.mulLocal(data.step!!.dtRatio) + + P.set(m_impulse.x, m_impulse.y) + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * (Vec2.cross(m_rA, P) + m_impulse.z) + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * (Vec2.cross(m_rB, P) + m_impulse.z) + pool.pushVec2(1) + } else { + m_impulse.setZero() + } + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(1) + pool.pushRot(2) + pool.pushMat33(1) + } + + override fun solveVelocityConstraints(data: SolverData) { + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val Cdot1 = pool.popVec2() + val P = pool.popVec2() + val temp = pool.popVec2() + if (frequency > 0.0f) { + val Cdot2 = wB - wA + + val impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z) + m_impulse.z += impulse2 + + wA -= iA * impulse2 + wB += iB * impulse2 + + Vec2.crossToOutUnsafe(wB, m_rB, Cdot1) + Vec2.crossToOutUnsafe(wA, m_rA, temp) + Cdot1.addLocal(vB).subLocal(vA).subLocal(temp) + + val impulse1 = P + Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1) + impulse1.negateLocal() + + m_impulse.x += impulse1.x + m_impulse.y += impulse1.y + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * Vec2.cross(m_rA, P) + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * Vec2.cross(m_rB, P) + } else { + Vec2.crossToOutUnsafe(wA, m_rA, temp) + Vec2.crossToOutUnsafe(wB, m_rB, Cdot1) + Cdot1.addLocal(vB).subLocal(vA).subLocal(temp) + val Cdot2 = wB - wA + + val Cdot = pool.popVec3() + Cdot.set(Cdot1.x, Cdot1.y, Cdot2) + + val impulse = pool.popVec3() + Mat33.mulToOutUnsafe(m_mass, Cdot, impulse) + impulse.negateLocal() + m_impulse.addLocal(impulse) + + P.set(impulse.x, impulse.y) + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * (Vec2.cross(m_rA, P) + impulse.z) + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * (Vec2.cross(m_rB, P) + impulse.z) + + pool.pushVec3(2) + } + + // data.velocities[m_indexA].v.set(vA); + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v.set(vB); + data.velocities!![m_indexB].w = wB + + pool.pushVec2(3) + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + val rA = pool.popVec2() + val rB = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), rB) + val positionError: Float + val angularError: Float + + val K = pool.popMat33() + val C1 = pool.popVec2() + val P = pool.popVec2() + + K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB + K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB + K.ez.x = -rA.y * iA - rB.y * iB + K.ex.y = K.ey.x + K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB + K.ez.y = rA.x * iA + rB.x * iB + K.ex.z = K.ez.x + K.ey.z = K.ez.y + K.ez.z = iA + iB + if (frequency > 0.0f) { + C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + + positionError = C1.length() + angularError = 0.0f + + K.solve22ToOut(C1, P) + P.negateLocal() + + cA.x -= mA * P.x + cA.y -= mA * P.y + aA -= iA * Vec2.cross(rA, P) + + cB.x += mB * P.x + cB.y += mB * P.y + aB += iB * Vec2.cross(rB, P) + } else { + C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + val C2 = aB - aA - referenceAngleRadians + + positionError = C1.length() + angularError = MathUtils.abs(C2) + + val C = pool.popVec3() + val impulse = pool.popVec3() + C.set(C1.x, C1.y, C2) + + K.solve33ToOut(C, impulse) + impulse.negateLocal() + P.set(impulse.x, impulse.y) + + cA.x -= mA * P.x + cA.y -= mA * P.y + aA -= iA * (Vec2.cross(rA, P) + impulse.z) + + cB.x += mB * P.x + cB.y += mB * P.y + aB += iB * (Vec2.cross(rB, P) + impulse.z) + pool.pushVec3(2) + } + + // data.positions[m_indexA].c.set(cA); + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c.set(cB); + data.positions!![m_indexB].a = aB + + pool.pushVec2(5) + pool.pushRot(2) + pool.pushMat33(1) + + return positionError <= Settings.linearSlop && angularError <= Settings.angularSlop + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJointDef.kt new file mode 100644 index 0000000..7290c18 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WeldJointDef.kt @@ -0,0 +1,97 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Created at 3:38:52 AM Jan 15, 2011 + */ + +/** + * @author Daniel Murphy + */ +class WeldJointDef : JointDef(JointType.WELD) { + /** + * The local anchor point relative to body1's origin. + */ + + val localAnchorA: Vec2 = Vec2() + + /** + * The local anchor point relative to body2's origin. + */ + + val localAnchorB: Vec2 = Vec2() + + /** + * The body2 angle minus body1 angle in the reference state (radians). + */ + var referenceAngleRadians: Float = 0f + + /** + * The body2 angle minus body1 angle in the reference state (degrees). + */ + var referenceAngleDegrees: Float + set(value) { referenceAngleRadians = value * MathUtils.DEG2RAD } + get() = referenceAngleRadians * MathUtils.RAD2DEG + + /** + * The body2 angle minus body1 angle in the reference state. + */ + var referenceAngle: Angle + set(value) { referenceAngleRadians = value.radians.toFloat() } + get() = referenceAngleRadians.radians + + /** + * The mass-spring-damper frequency in Hertz. Rotation only. Disable softness with a value of 0. + */ + + var frequencyHz: Float = 0f + + /** + * The damping ratio. 0 = no damping, 1 = critical damping. + */ + + var dampingRatio: Float = 0f + + /** + * Initialize the bodies, anchors, and reference angle using a world anchor point. + * + * @param bA + * @param bB + * @param anchor + */ + fun initialize(bA: Body, bB: Body, anchor: Vec2) { + bodyA = bA + bodyB = bB + bodyA!!.getLocalPointToOut(anchor, localAnchorA) + bodyB!!.getLocalPointToOut(anchor, localAnchorB) + referenceAngleRadians = bodyB!!.angleRadians - bodyA!!.angleRadians + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJoint.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJoint.kt new file mode 100644 index 0000000..c9646c9 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJoint.kt @@ -0,0 +1,464 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.SolverData +import org.jbox2d.pooling.IWorldPool + +//Linear constraint (point-to-line) +//d = pB - pA = xB + rB - xA - rA +//C = dot(ay, d) +//Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +//J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + +//Spring linear constraint +//C = dot(ax, d) +//Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) +//J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + +//Motor rotational constraint +//Cdot = wB - wA +//J = [0 0 -1 0 0 1] + +/** + * A wheel joint. This joint provides two degrees of freedom: translation along an axis fixed in + * bodyA and rotation in the plane. You can use a joint limit to restrict the range of motion and a + * joint motor to drive the rotation or to model rotational friction. This joint is designed for + * vehicle suspensions. + * + * @author Daniel Murphy + */ +class WheelJoint(argPool: IWorldPool, def: WheelJointDef) : Joint(argPool, def) { + + var springFrequencyHz: Float = 0.toFloat() + + var springDampingRatio: Float = 0.toFloat() + + // Solver shared + + val localAnchorA = Vec2() + + val localAnchorB = Vec2() + /** For serialization */ + + val localAxisA = Vec2() + private val m_localYAxisA = Vec2() + + private var m_impulse: Float = 0.toFloat() + private var m_motorImpulse: Float = 0.toFloat() + private var m_springImpulse: Float = 0.toFloat() + + private var m_maxMotorTorque: Float = 0.toFloat() + private var m_motorSpeed: Float = 0.toFloat() + var isMotorEnabled: Boolean = false + private set + + // Solver temp + private var m_indexA: Int = 0 + private var m_indexB: Int = 0 + private val m_localCenterA = Vec2() + private val m_localCenterB = Vec2() + private var m_invMassA: Float = 0.toFloat() + private var m_invMassB: Float = 0.toFloat() + private var m_invIA: Float = 0.toFloat() + private var m_invIB: Float = 0.toFloat() + + private val m_ax = Vec2() + private val m_ay = Vec2() + private var m_sAx: Float = 0.toFloat() + private var m_sBx: Float = 0.toFloat() + private var m_sAy: Float = 0.toFloat() + private var m_sBy: Float = 0.toFloat() + + private var m_mass: Float = 0.toFloat() + private var m_motorMass: Float = 0.toFloat() + private var m_springMass: Float = 0.toFloat() + + private var m_bias: Float = 0.toFloat() + private var m_gamma: Float = 0.toFloat() + + val jointTranslation: Float + get() { + val b1 = bodyA + val b2 = bodyB + + val p1 = pool.popVec2() + val p2 = pool.popVec2() + val axis = pool.popVec2() + b1!!.getWorldPointToOut(localAnchorA, p1) + b2!!.getWorldPointToOut(localAnchorA, p2) + p2.subLocal(p1) + b1.getWorldVectorToOut(localAxisA, axis) + + val translation = Vec2.dot(p2, axis) + pool.pushVec2(3) + return translation + } + + val jointSpeed: Float + get() = bodyA!!._angularVelocity - bodyB!!._angularVelocity + + var motorSpeed: Float + get() = m_motorSpeed + set(speed) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_motorSpeed = speed + } + + var maxMotorTorque: Float + get() = m_maxMotorTorque + set(torque) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + m_maxMotorTorque = torque + } + + // pooling + private val rA = Vec2() + private val rB = Vec2() + private val d = Vec2() + + init { + localAnchorA.set(def.localAnchorA) + localAnchorB.set(def.localAnchorB) + localAxisA.set(def.localAxisA) + Vec2.crossToOutUnsafe(1.0f, localAxisA, m_localYAxisA) + + + m_motorMass = 0.0f + m_motorImpulse = 0.0f + + m_maxMotorTorque = def.maxMotorTorque + m_motorSpeed = def.motorSpeed + isMotorEnabled = def.enableMotor + + springFrequencyHz = def.frequencyHz + springDampingRatio = def.dampingRatio + } + + override fun getAnchorA(argOut: Vec2) { + bodyA!!.getWorldPointToOut(localAnchorA, argOut) + } + + override fun getAnchorB(argOut: Vec2) { + bodyB!!.getWorldPointToOut(localAnchorB, argOut) + } + + override fun getReactionForce(inv_dt: Float, argOut: Vec2) { + val temp = pool.popVec2() + temp.set(m_ay).mulLocal(m_impulse) + argOut.set(m_ax).mulLocal(m_springImpulse).addLocal(temp).mulLocal(inv_dt) + pool.pushVec2(1) + } + + override fun getReactionTorque(inv_dt: Float): Float { + return inv_dt * m_motorImpulse + } + + fun enableMotor(flag: Boolean) { + bodyA!!.isAwake = true + bodyB!!.isAwake = true + isMotorEnabled = flag + } + + fun getMotorTorque(inv_dt: Float): Float { + return m_motorImpulse * inv_dt + } + + override fun initVelocityConstraints(data: SolverData) { + m_indexA = bodyA!!.islandIndex + m_indexB = bodyB!!.islandIndex + m_localCenterA.set(bodyA!!.sweep.localCenter) + m_localCenterB.set(bodyB!!.sweep.localCenter) + m_invMassA = bodyA!!.m_invMass + m_invMassB = bodyB!!.m_invMass + m_invIA = bodyA!!.m_invI + m_invIB = bodyB!!.m_invI + + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val cA = data.positions!![m_indexA].c + val aA = data.positions!![m_indexA].a + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + + val cB = data.positions!![m_indexB].c + val aB = data.positions!![m_indexB].a + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + // Compute the effective masses. + Rot.mulToOutUnsafe(qA, temp.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOutUnsafe(qB, temp.set(localAnchorB).subLocal(m_localCenterB), rB) + d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA) + + // Point to line constraint + run { + Rot.mulToOut(qA, m_localYAxisA, m_ay) + m_sAy = Vec2.cross(temp.set(d).addLocal(rA), m_ay) + m_sBy = Vec2.cross(rB, m_ay) + + m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy + + if (m_mass > 0.0f) { + m_mass = 1.0f / m_mass + } + } + + // Spring constraint + m_springMass = 0.0f + m_bias = 0.0f + m_gamma = 0.0f + if (springFrequencyHz > 0.0f) { + Rot.mulToOut(qA, localAxisA, m_ax) + m_sAx = Vec2.cross(temp.set(d).addLocal(rA), m_ax) + m_sBx = Vec2.cross(rB, m_ax) + + val invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx + + if (invMass > 0.0f) { + m_springMass = 1.0f / invMass + + val C = Vec2.dot(d, m_ax) + + // Frequency + val omega = 2.0f * MathUtils.PI * springFrequencyHz + + // Damping coefficient + val d = 2.0f * m_springMass * springDampingRatio * omega + + // Spring stiffness + val k = m_springMass * omega * omega + + // magic formulas + val h = data.step!!.dt + m_gamma = h * (d + h * k) + if (m_gamma > 0.0f) { + m_gamma = 1.0f / m_gamma + } + + m_bias = C * h * k * m_gamma + + m_springMass = invMass + m_gamma + if (m_springMass > 0.0f) { + m_springMass = 1.0f / m_springMass + } + } + } else { + m_springImpulse = 0.0f + } + + // Rotational motor + if (isMotorEnabled) { + m_motorMass = iA + iB + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass + } + } else { + m_motorMass = 0.0f + m_motorImpulse = 0.0f + } + + if (data.step!!.warmStarting) { + val P = pool.popVec2() + // Account for variable time step. + m_impulse *= data.step!!.dtRatio + m_springImpulse *= data.step!!.dtRatio + m_motorImpulse *= data.step!!.dtRatio + + P.x = m_impulse * m_ay.x + m_springImpulse * m_ax.x + P.y = m_impulse * m_ay.y + m_springImpulse * m_ax.y + val LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse + val LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse + + vA.x -= m_invMassA * P.x + vA.y -= m_invMassA * P.y + wA -= m_invIA * LA + + vB.x += m_invMassB * P.x + vB.y += m_invMassB * P.y + wB += m_invIB * LB + pool.pushVec2(1) + } else { + m_impulse = 0.0f + m_springImpulse = 0.0f + m_motorImpulse = 0.0f + } + pool.pushRot(2) + pool.pushVec2(1) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + } + + override fun solveVelocityConstraints(data: SolverData) { + val mA = m_invMassA + val mB = m_invMassB + val iA = m_invIA + val iB = m_invIB + + val vA = data.velocities!![m_indexA].v + var wA = data.velocities!![m_indexA].w + val vB = data.velocities!![m_indexB].v + var wB = data.velocities!![m_indexB].w + + val temp = pool.popVec2() + val P = pool.popVec2() + + // Solve spring constraint + run { + val Cdot = Vec2.dot(m_ax, temp.set(vB).subLocal(vA)) + m_sBx * wB - m_sAx * wA + val impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse) + m_springImpulse += impulse + + P.x = impulse * m_ax.x + P.y = impulse * m_ax.y + val LA = impulse * m_sAx + val LB = impulse * m_sBx + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + } + + // Solve rotational motor constraint + run { + val Cdot = wB - wA - m_motorSpeed + var impulse = -m_motorMass * Cdot + + val oldImpulse = m_motorImpulse + val maxImpulse = data.step!!.dt * m_maxMotorTorque + m_motorImpulse = MathUtils.clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse) + impulse = m_motorImpulse - oldImpulse + + wA -= iA * impulse + wB += iB * impulse + } + + // Solve point to line constraint + run { + val Cdot = Vec2.dot(m_ay, temp.set(vB).subLocal(vA)) + m_sBy * wB - m_sAy * wA + val impulse = -m_mass * Cdot + m_impulse += impulse + + P.x = impulse * m_ay.x + P.y = impulse * m_ay.y + val LA = impulse * m_sAy + val LB = impulse * m_sBy + + vA.x -= mA * P.x + vA.y -= mA * P.y + wA -= iA * LA + + vB.x += mB * P.x + vB.y += mB * P.y + wB += iB * LB + } + pool.pushVec2(2) + + // data.velocities[m_indexA].v = vA; + data.velocities!![m_indexA].w = wA + // data.velocities[m_indexB].v = vB; + data.velocities!![m_indexB].w = wB + } + + override fun solvePositionConstraints(data: SolverData): Boolean { + val cA = data.positions!![m_indexA].c + var aA = data.positions!![m_indexA].a + val cB = data.positions!![m_indexB].c + var aB = data.positions!![m_indexB].a + + val qA = pool.popRot() + val qB = pool.popRot() + val temp = pool.popVec2() + + qA.setRadians(aA) + qB.setRadians(aB) + + Rot.mulToOut(qA, temp.set(localAnchorA).subLocal(m_localCenterA), rA) + Rot.mulToOut(qB, temp.set(localAnchorB).subLocal(m_localCenterB), rB) + d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA) + + val ay = pool.popVec2() + Rot.mulToOut(qA, m_localYAxisA, ay) + + val sAy = Vec2.cross(temp.set(d).addLocal(rA), ay) + val sBy = Vec2.cross(rB, ay) + + val C = Vec2.dot(d, ay) + + val k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy + + val impulse: Float + if (k != 0.0f) { + impulse = -C / k + } else { + impulse = 0.0f + } + + val P = pool.popVec2() + P.x = impulse * ay.x + P.y = impulse * ay.y + val LA = impulse * sAy + val LB = impulse * sBy + + cA.x -= m_invMassA * P.x + cA.y -= m_invMassA * P.y + aA -= m_invIA * LA + cB.x += m_invMassB * P.x + cB.y += m_invMassB * P.y + aB += m_invIB * LB + + pool.pushVec2(3) + pool.pushRot(2) + // data.positions[m_indexA].c = cA; + data.positions!![m_indexA].a = aA + // data.positions[m_indexB].c = cB; + data.positions!![m_indexB].a = aB + + return MathUtils.abs(C) <= Settings.linearSlop + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJointDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJointDef.kt new file mode 100644 index 0000000..9f3977f --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/dynamics/joints/WheelJointDef.kt @@ -0,0 +1,98 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 7:27:31 AM Jan 21, 2011 + */ +package org.jbox2d.dynamics.joints + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +/** + * Wheel joint definition. This requires defining a line of motion using an axis and an anchor + * point. The definition uses local anchor points and a local axis so that the initial configuration + * can violate the constraint slightly. The joint translation is zero when the local anchor points + * coincide in world space. Using local anchors and a local axis helps when saving and loading a + * game. + * + * @author Daniel Murphy + */ +class WheelJointDef : JointDef(JointType.WHEEL) { + + /** + * The local anchor point relative to body1's origin. + */ + + val localAnchorA = Vec2() + + /** + * The local anchor point relative to body2's origin. + */ + + val localAnchorB = Vec2() + + /** + * The local translation axis in body1. + */ + + val localAxisA = Vec2(1f, 0f) + + /** + * Enable/disable the joint motor. + */ + + var enableMotor: Boolean = false + + /** + * The maximum motor torque, usually in N-m. + */ + + var maxMotorTorque: Float = 0f + + /** + * The desired motor speed in radians per second. + */ + + var motorSpeed: Float = 0f + + /** + * Suspension frequency, zero indicates no suspension + */ + + var frequencyHz: Float = 0f + + /** + * Suspension damping ratio, one indicates critical damping + */ + + var dampingRatio: Float = 0.toFloat() + + fun initialize(b1: Body, b2: Body, anchor: Vec2, axis: Vec2) { + bodyA = b1 + bodyB = b2 + b1.getLocalPointToOut(anchor, localAnchorA) + b2.getLocalPointToOut(anchor, localAnchorB) + bodyA!!.getLocalVectorToOut(axis, localAxisA) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/internal/internal.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/internal/internal.kt new file mode 100644 index 0000000..5e89d51 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/internal/internal.kt @@ -0,0 +1,38 @@ +package org.jbox2d.internal + +internal inline fun arraycopy(src: Array, srcPos: Int, dst: Array, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: BooleanArray, srcPos: Int, dst: BooleanArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: LongArray, srcPos: Int, dst: LongArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: ByteArray, srcPos: Int, dst: ByteArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: ShortArray, srcPos: Int, dst: ShortArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: IntArray, srcPos: Int, dst: IntArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: FloatArray, srcPos: Int, dst: FloatArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun arraycopy(src: DoubleArray, srcPos: Int, dst: DoubleArray, dstPos: Int, size: Int): Unit = + run { src.copyInto(dst, dstPos, srcPos, srcPos + size) } + +internal inline fun assert(boolean: Boolean) = check(boolean) +internal inline fun assert(boolean: Boolean, message: () -> String) = check(boolean) + +internal fun > Arrays_sort(array: Array, fromIndex: Int, toIndex: Int) { + val sorted = array.copyOfRange(fromIndex, toIndex).sortedArray() + arraycopy(sorted, 0, array, fromIndex, toIndex - fromIndex) +} + +internal fun Arrays_sort(array: LongArray, fromIndex: Int, toIndex: Int) { + val sorted = array.copyOfRange(fromIndex, toIndex).sortedArray() + arraycopy(sorted, 0, array, fromIndex, toIndex - fromIndex) +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleBodyContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleBodyContact.kt new file mode 100644 index 0000000..b3d6b65 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleBodyContact.kt @@ -0,0 +1,22 @@ +package org.jbox2d.particle + +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Body + +class ParticleBodyContact { + /** Index of the particle making contact. */ + + var index: Int = 0 + /** The body making contact. */ + + var body: Body? = null + /** Weight of the contact. A value between 0.0f and 1.0f. */ + + internal var weight: Float = 0.toFloat() + /** The normalized direction from the particle to the body. */ + + val normal = Vec2() + /** The effective mass used in calculating force. */ + + internal var mass: Float = 0.toFloat() +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleColor.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleColor.kt new file mode 100644 index 0000000..fade6cc --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleColor.kt @@ -0,0 +1,58 @@ +package org.jbox2d.particle + +import org.jbox2d.common.Color3f + +/** + * Small color object for each particle + * + * @author dmurph + */ +class ParticleColor { + + var r: Byte = 0 + + var g: Byte = 0 + + var b: Byte = 0 + + var a: Byte = 0 + + val isZero: Boolean + get() = r.toInt() == 0 && g.toInt() == 0 && b.toInt() == 0 && a.toInt() == 0 + + constructor() { + r = 127.toByte() + g = 127.toByte() + b = 127.toByte() + a = 50.toByte() + } + + constructor(r: Byte, g: Byte, b: Byte, a: Byte) { + set(r, g, b, a) + } + + constructor(color: Color3f) { + set(color) + } + + fun set(color: Color3f) { + r = (255 * color.x).toInt().toByte() + g = (255 * color.y).toInt().toByte() + b = (255 * color.z).toInt().toByte() + a = 255.toByte() + } + + fun set(color: ParticleColor) { + r = color.r + g = color.g + b = color.b + a = color.a + } + + fun set(r: Byte, g: Byte, b: Byte, a: Byte) { + this.r = r + this.g = g + this.b = b + this.a = a + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleContact.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleContact.kt new file mode 100644 index 0000000..a7642a2 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleContact.kt @@ -0,0 +1,20 @@ +package org.jbox2d.particle + +import org.jbox2d.common.Vec2 + +class ParticleContact { + /** Indices of the respective particles making contact. */ + + var indexA: Int = 0 + + var indexB: Int = 0 + /** The logical sum of the particle behaviors that have been set. */ + + var flags: Int = 0 + /** Weight of the contact. A value between 0.0f and 1.0f. */ + + var weight: Float = 0.toFloat() + /** The normalized direction from A to B. */ + + val normal = Vec2() +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleDef.kt new file mode 100644 index 0000000..87f4258 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleDef.kt @@ -0,0 +1,30 @@ +package org.jbox2d.particle + +import org.jbox2d.common.Vec2 +import org.jbox2d.userdata.Box2dTypedUserData + +class ParticleDef : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + /** + * Specifies the type of particle. A particle may be more than one type. Multiple types are + * chained by logical sums, for example: pd.flags = ParticleType.b2_elasticParticle | + * ParticleType.b2_viscousParticle. + */ + + internal var flags: Int = 0 + + /** The world position of the particle. */ + + val position = Vec2() + + /** The linear velocity of the particle in world co-ordinates. */ + + val velocity = Vec2() + + /** The color of the particle. */ + + var color: ParticleColor? = null + + /** Use this to store application-specific body data. */ + + var userData: Any? = null +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroup.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroup.kt new file mode 100644 index 0000000..46ef52b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroup.kt @@ -0,0 +1,148 @@ +package org.jbox2d.particle + +import korlibs.math.geom.Angle +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 + +class ParticleGroup { + + internal var m_system: ParticleSystem? = null + + var m_firstIndex: Int = 0 + + internal var m_lastIndex: Int = 0 + + var m_groupFlags: Int = 0 + + internal var m_strength: Float = 0.toFloat() + + internal var m_prev: ParticleGroup? = null + + var m_next: ParticleGroup? = null + + fun getNext() = m_next + + + internal var m_timestamp: Int = 0 + + internal var m_mass: Float = 0.toFloat() + + internal var m_inertia: Float = 0.toFloat() + + internal val m_center = Vec2() + + internal val m_linearVelocity = Vec2() + + internal var m_angularVelocity: Float = 0.toFloat() + + val m_transform = Transform() + + + internal var m_destroyAutomatically: Boolean = false + + internal var m_toBeDestroyed: Boolean = false + + internal var m_toBeSplit: Boolean = false + + + var m_userData: Any? = null + + val particleCount: Int + get() = m_lastIndex - m_firstIndex + + val mass: Float + get() { + updateStatistics() + return m_mass + } + + val inertia: Float + get() { + updateStatistics() + return m_inertia + } + + val center: Vec2 + get() { + updateStatistics() + return m_center + } + + val linearVelocity: Vec2 + get() { + updateStatistics() + return m_linearVelocity + } + + val angularVelocity: Float + get() { + updateStatistics() + return m_angularVelocity + } + + val position: Vec2 + get() = m_transform.p + + val angleRadians: Float get() = m_transform.q.angleRadians + val angleDegrees: Float get() = m_transform.q.angleDegrees + val angle: Angle get() = m_transform.q.angle + + init { + // m_system = null; + m_firstIndex = 0 + m_lastIndex = 0 + m_groupFlags = 0 + m_strength = 1.0f + + m_timestamp = -1 + m_mass = 0f + m_inertia = 0f + m_angularVelocity = 0f + m_transform.setIdentity() + + m_destroyAutomatically = true + m_toBeDestroyed = false + m_toBeSplit = false + } + + + fun updateStatistics() { + if (m_timestamp != m_system!!.m_timestamp) { + val m = m_system!!.particleMass + m_mass = 0f + m_center.setZero() + m_linearVelocity.setZero() + for (i in m_firstIndex until m_lastIndex) { + m_mass += m + val pos = m_system!!.m_positionBuffer!!.data!![i] + m_center.x += m * pos.x + m_center.y += m * pos.y + val vel = m_system!!.m_velocityBuffer.data!![i] + m_linearVelocity.x += m * vel.x + m_linearVelocity.y += m * vel.y + } + if (m_mass > 0) { + m_center.x *= 1 / m_mass + m_center.y *= 1 / m_mass + m_linearVelocity.x *= 1 / m_mass + m_linearVelocity.y *= 1 / m_mass + } + m_inertia = 0f + m_angularVelocity = 0f + for (i in m_firstIndex until m_lastIndex) { + val pos = m_system!!.m_positionBuffer.data!![i] + val vel = m_system!!.m_velocityBuffer.data!![i] + val px = pos.x - m_center.x + val py = pos.y - m_center.y + val vx = vel.x - m_linearVelocity.x + val vy = vel.y - m_linearVelocity.y + m_inertia += m * (px * px + py * py) + m_angularVelocity += m * (px * vy - py * vx) + } + if (m_inertia > 0) { + m_angularVelocity *= 1 / m_inertia + } + m_timestamp = m_system!!.m_timestamp + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupDef.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupDef.kt new file mode 100644 index 0000000..d1123e2 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupDef.kt @@ -0,0 +1,83 @@ +package org.jbox2d.particle + +import korlibs.math.geom.Angle +import korlibs.math.geom.radians +import org.jbox2d.collision.shapes.Shape +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.userdata.Box2dTypedUserData + +/** + * A particle group definition holds all the data needed to construct a particle group. You can + * safely re-use these definitions. + */ +class ParticleGroupDef : Box2dTypedUserData by Box2dTypedUserData.Mixin() { + + /** The particle-behavior flags. */ + + var flags: Int = 0 + + /** The group-construction flags. */ + + var groupFlags: Int = 0 + + /** + * The world position of the group. Moves the group's shape a distance equal to the value of + * position. + */ + + val position = Vec2() + + /** + * The world angle of the group in radians. Rotates the shape by an angle equal to the value of + * angle. + */ + var angleRadians: Float = 0f + + /** + * The world angle of the group in degrees. Rotates the shape by an angle equal to the value of + * angle. + */ + var angleDegrees: Float + set(value) { angleRadians = value * MathUtils.DEG2RAD } + get() = angleRadians * MathUtils.RAD2DEG + + /** + * The world angle of the group. Rotates the shape by an angle equal to the value of + * angle. + */ + var angle: Angle + set(value) { angleRadians = value.radians.toFloat() } + get() = angleRadians.radians + + /** The linear velocity of the group's origin in world co-ordinates. */ + + val linearVelocity = Vec2() + + /** The angular velocity of the group. */ + + var angularVelocity: Float = 0f + + /** The color of all particles in the group. */ + + var color: ParticleColor? = null + + /** + * The strength of cohesion among the particles in a group with flag b2_elasticParticle or + * b2_springParticle. + */ + + var strength: Float = 1f + + /** Shape containing the particle group. */ + + var shape: Shape? = null + + /** If true, destroy the group automatically after its last particle has been destroyed. */ + + var destroyAutomatically: Boolean = true + + /** Use this to store application-specific group data. */ + + var userData: Any? = null +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupType.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupType.kt new file mode 100644 index 0000000..ec05539 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleGroupType.kt @@ -0,0 +1,10 @@ +package org.jbox2d.particle + +object ParticleGroupType { + /** resists penetration */ + + val b2_solidParticleGroup = 1 shl 0 + /** keeps its shape */ + + val b2_rigidParticleGroup = 1 shl 1 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleSystem.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleSystem.kt new file mode 100644 index 0000000..5492878 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleSystem.kt @@ -0,0 +1,2071 @@ +package org.jbox2d.particle + +import org.jbox2d.callbacks.ParticleQueryCallback +import org.jbox2d.callbacks.ParticleRaycastCallback +import org.jbox2d.callbacks.QueryCallback +import org.jbox2d.collision.AABB +import org.jbox2d.collision.RayCastInput +import org.jbox2d.collision.RayCastOutput +import org.jbox2d.collision.shapes.Shape +import org.jbox2d.common.BufferUtils +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Transform +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.Fixture +import org.jbox2d.dynamics.TimeStep +import org.jbox2d.dynamics.World +import org.jbox2d.internal.Arrays_sort +import org.jbox2d.internal.assert +import org.jbox2d.particle.VoronoiDiagram.VoronoiDiagramCallback + +class ParticleSystem(internal var m_world: World) { + + internal var m_timestamp: Int = 0 + internal var m_allParticleFlags: Int = 0 + internal var m_allGroupFlags: Int = 0 + internal var m_density: Float = 1f + internal var m_inverseDensity: Float = 1f + var particleGravityScale: Float = 1f + internal var m_particleDiameter: Float = 1f + internal var m_inverseDiameter: Float = 1f + internal var m_squaredDiameter: Float = 1f + + var particleCount: Int = 0 + internal set + internal var m_internalAllocatedCapacity: Int = 0 + internal var m_maxCount: Int = 0 + internal var m_flagsBuffer: ParticleBufferInt = ParticleBufferInt() + internal var m_positionBuffer: ParticleBuffer = ParticleBuffer { Vec2() } + internal var m_velocityBuffer: ParticleBuffer = ParticleBuffer { Vec2() } + internal var m_accumulationBuffer: FloatArray = FloatArray(0) // temporary values + internal var m_accumulation2Buffer: Array = emptyArray() // temporary vector values + internal var m_depthBuffer: FloatArray? = null // distance from the surface + + var m_colorBuffer: ParticleBuffer = ParticleBuffer { ParticleColor() } + var particleGroupBuffer: Array = emptyArray() + internal set + internal var m_userDataBuffer: ParticleBuffer = ParticleBuffer { Any() } + + internal var m_proxyCount: Int = 0 + internal var m_proxyCapacity: Int = 0 + internal var m_proxyBuffer: Array = emptyArray() + + var m_contactCount: Int = 0 + internal var m_contactCapacity: Int = 0 + var m_contactBuffer: Array = emptyArray() + + var m_bodyContactCount: Int = 0 + internal var m_bodyContactCapacity: Int = 0 + var m_bodyContactBuffer: Array = emptyArray() + + internal var m_pairCount: Int = 0 + internal var m_pairCapacity: Int = 0 + internal var m_pairBuffer: Array = emptyArray() + + internal var m_triadCount: Int = 0 + internal var m_triadCapacity: Int = 0 + internal var m_triadBuffer: Array = emptyArray() + + var particleGroupCount: Int = 0 + internal set + internal var m_groupList: ParticleGroup? = null + + internal var m_pressureStrength: Float = 0.05f + var particleDamping: Float = 1f + internal var m_elasticStrength: Float = 0.25f + internal var m_springStrength: Float = 0.25f + internal var m_viscousStrength: Float = 0.25f + internal var m_surfaceTensionStrengthA: Float = 0.1f + internal var m_surfaceTensionStrengthB: Float = 0.2f + internal var m_powderStrength: Float = 0.5f + internal var m_ejectionStrength: Float = 0.5f + internal var m_colorMixingStrength: Float = 0.5f + + private val temp = AABB() + private val dpcallback = DestroyParticlesInShapeCallback() + + private val temp2 = AABB() + private val tempVec = Vec2() + private val tempTransform = Transform() + private val tempTransform2 = Transform() + private val createParticleGroupCallback = CreateParticleGroupCallback() + private val tempParticleDef = ParticleDef() + + private val ubccallback = UpdateBodyContactsCallback() + + private val sccallback = SolveCollisionCallback() + + private val tempVec2 = Vec2() + private val tempRot = Rot() + private val tempXf = Transform() + private val tempXf2 = Transform() + + private val newIndices = NewIndices() + + var particleDensity: Float + get() = m_density + set(density) { + m_density = density + m_inverseDensity = 1 / m_density + } + + var particleRadius: Float + get() = m_particleDiameter / 2 + set(radius) { + m_particleDiameter = 2 * radius + m_squaredDiameter = m_particleDiameter * m_particleDiameter + m_inverseDiameter = 1 / m_particleDiameter + } + + internal val particleStride: Float + get() = Settings.particleStride * m_particleDiameter + + internal val particleMass: Float + get() { + val stride = particleStride + return m_density * stride * stride + } + + internal val particleInvMass: Float + get() = 1.777777f * m_inverseDensity * m_inverseDiameter * m_inverseDiameter + + val particleFlagsBuffer: IntArray? + get() = m_flagsBuffer.data + + val particlePositionBuffer: Array? + get() = m_positionBuffer.data + + val particleVelocityBuffer: Array? + get() = m_velocityBuffer.data + + val particleColorBuffer: Array? + get() { + m_colorBuffer.data = requestParticleBuffer({ ParticleColor() }, m_colorBuffer.data) + return m_colorBuffer.data + } + + val particleUserDataBuffer: Array? + get() { + m_userDataBuffer.data = requestParticleBuffer({ Any() }, m_userDataBuffer.data) + return m_userDataBuffer.data + } + + var particleMaxCount: Int + get() = m_maxCount + set(count) { + assert(particleCount <= count) + m_maxCount = count + } + + // public void assertNotSamePosition() { + // for (int i = 0; i < m_count; i++) { + // Vec2 vi = m_positionBuffer.data[i]; + // for (int j = i + 1; j < m_count; j++) { + // Vec2 vj = m_positionBuffer.data[j]; + // assert(vi.x != vj.x || vi.y != vj.y); + // } + // } + // } + + fun createParticle(def: ParticleDef): Int { + if (particleCount >= m_internalAllocatedCapacity) { + var capacity = if (particleCount != 0) 2 * particleCount else Settings.minParticleBufferCapacity + capacity = limitCapacity(capacity, m_maxCount) + capacity = limitCapacity(capacity, m_flagsBuffer.userSuppliedCapacity) + capacity = limitCapacity(capacity, m_positionBuffer.userSuppliedCapacity) + capacity = limitCapacity(capacity, m_velocityBuffer.userSuppliedCapacity) + capacity = limitCapacity(capacity, m_colorBuffer.userSuppliedCapacity) + capacity = limitCapacity(capacity, m_userDataBuffer.userSuppliedCapacity) + if (m_internalAllocatedCapacity < capacity) { + m_flagsBuffer.data = reallocateBuffer(m_flagsBuffer, m_internalAllocatedCapacity, capacity, false) + m_positionBuffer.data = reallocateBuffer(m_positionBuffer, m_internalAllocatedCapacity, capacity, false) + m_velocityBuffer.data = reallocateBuffer(m_velocityBuffer, m_internalAllocatedCapacity, capacity, false) + m_accumulationBuffer = BufferUtils.reallocateBuffer(m_accumulationBuffer, 0, m_internalAllocatedCapacity, capacity, false) + m_accumulation2Buffer = BufferUtils.reallocateBuffer({ Vec2() }, m_accumulation2Buffer, 0, m_internalAllocatedCapacity, capacity, true) + m_depthBuffer = BufferUtils.reallocateBuffer(m_depthBuffer, 0, m_internalAllocatedCapacity, capacity, true) + m_colorBuffer.data = reallocateBuffer(m_colorBuffer, m_internalAllocatedCapacity, capacity, true) + particleGroupBuffer = BufferUtils.reallocateBuffer({ ParticleGroup() }, particleGroupBuffer as Array, 0, m_internalAllocatedCapacity, capacity, false) as Array + m_userDataBuffer.data = reallocateBuffer(m_userDataBuffer, m_internalAllocatedCapacity, capacity, true) + m_internalAllocatedCapacity = capacity + } + } + if (particleCount >= m_internalAllocatedCapacity) { + return Settings.invalidParticleIndex + } + val index = particleCount++ + m_flagsBuffer!!.data!![index] = def.flags + m_positionBuffer.data!![index].set(def.position) + // assertNotSamePosition(); + m_velocityBuffer.data!![index].set(def.velocity) + particleGroupBuffer!![index] = null + if (m_depthBuffer != null) { + m_depthBuffer!![index] = 0f + } + if (m_colorBuffer.data != null || def.color != null) { + m_colorBuffer.data = requestParticleBuffer(m_colorBuffer.dataClass, m_colorBuffer.data) + m_colorBuffer.data!![index].set(def.color!!) + } + if (m_userDataBuffer.data != null || def.userData != null) { + m_userDataBuffer.data = requestParticleBuffer(m_userDataBuffer.dataClass, m_userDataBuffer.data) + m_userDataBuffer.data!![index] = def.userData!! + } + if (m_proxyCount >= m_proxyCapacity) { + val oldCapacity = m_proxyCapacity + val newCapacity = if (m_proxyCount != 0) 2 * m_proxyCount else Settings.minParticleBufferCapacity + m_proxyBuffer = BufferUtils.reallocateBuffer({ Proxy() }, m_proxyBuffer, oldCapacity, newCapacity) + m_proxyCapacity = newCapacity + } + m_proxyBuffer[m_proxyCount++].index = index + return index + } + + fun destroyParticle(index: Int, callDestructionListener: Boolean) { + var flags = ParticleType.b2_zombieParticle + if (callDestructionListener) { + flags = flags or ParticleType.b2_destructionListener + } + m_flagsBuffer!!.data!![index] = m_flagsBuffer!!.data!![index] or flags + } + + fun destroyParticlesInShape(shape: Shape, xf: Transform, callDestructionListener: Boolean): Int { + dpcallback.init(this, shape, xf, callDestructionListener) + shape.computeAABB(temp, xf, 0) + m_world.queryAABB(dpcallback, temp) + return dpcallback.destroyed + } + + fun destroyParticlesInGroup(group: ParticleGroup, callDestructionListener: Boolean) { + for (i in group.m_firstIndex until group.m_lastIndex) { + destroyParticle(i, callDestructionListener) + } + } + + fun createParticleGroup(groupDef: ParticleGroupDef): ParticleGroup { + val stride = particleStride + val identity = tempTransform + identity.setIdentity() + val transform = tempTransform2 + transform.setIdentity() + val firstIndex = particleCount + if (groupDef.shape != null) { + val particleDef = tempParticleDef + particleDef.flags = groupDef.flags + particleDef.color = groupDef.color + particleDef.userData = groupDef.userData + val shape = groupDef.shape + transform.setRadians(groupDef.position, groupDef.angleRadians) + val aabb = temp + val childCount = shape!!.getChildCount() + for (childIndex in 0 until childCount) { + if (childIndex == 0) { + shape.computeAABB(aabb, identity, childIndex) + } else { + val childAABB = temp2 + shape.computeAABB(childAABB, identity, childIndex) + aabb.combine(childAABB) + } + } + val upperBoundY = aabb.upperBound.y + val upperBoundX = aabb.upperBound.x + var y = MathUtils.floor(aabb.lowerBound.y / stride) * stride + while (y < upperBoundY) { + var x = MathUtils.floor(aabb.lowerBound.x / stride) * stride + while (x < upperBoundX) { + val p = tempVec + p.x = x + p.y = y + if (shape.testPoint(identity, p)) { + Transform.mulToOut(transform, p, p) + particleDef.position.x = p.x + particleDef.position.y = p.y + p.subLocal(groupDef.position) + Vec2.crossToOutUnsafe(groupDef.angularVelocity, p, particleDef.velocity) + particleDef.velocity.addLocal(groupDef.linearVelocity) + createParticle(particleDef) + } + x += stride + } + y += stride + } + } + val lastIndex = particleCount + + val group = ParticleGroup() + group.m_system = this + group.m_firstIndex = firstIndex + group.m_lastIndex = lastIndex + group.m_groupFlags = groupDef.groupFlags + group.m_strength = groupDef.strength + group.m_userData = groupDef.userData + group.m_transform.set(transform) + group.m_destroyAutomatically = groupDef.destroyAutomatically + group.m_prev = null + group.m_next = m_groupList + if (m_groupList != null) { + m_groupList!!.m_prev = group + } + m_groupList = group + ++particleGroupCount + for (i in firstIndex until lastIndex) { + particleGroupBuffer!![i] = group + } + + updateContacts(true) + if (groupDef.flags and k_pairFlags != 0) { + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + var a = contact.indexA + var b = contact.indexB + if (a > b) { + val temp = a + a = b + b = temp + } + if (firstIndex <= a && b < lastIndex) { + if (m_pairCount >= m_pairCapacity) { + val oldCapacity = m_pairCapacity + val newCapacity = if (m_pairCount != 0) 2 * m_pairCount else Settings.minParticleBufferCapacity + m_pairBuffer = BufferUtils.reallocateBuffer({ Pair() }, m_pairBuffer, oldCapacity, newCapacity) + m_pairCapacity = newCapacity + } + val pair = m_pairBuffer[m_pairCount] + pair.indexA = a + pair.indexB = b + pair.flags = contact.flags + pair.strength = groupDef.strength + pair.distance = MathUtils.distance(m_positionBuffer.data!![a], m_positionBuffer.data!![b]) + m_pairCount++ + } + } + } + if (groupDef.flags and k_triadFlags != 0) { + val diagram = VoronoiDiagram(lastIndex - firstIndex) + for (i in firstIndex until lastIndex) { + diagram.addGenerator(m_positionBuffer.data!![i], i) + } + diagram.generate(stride / 2) + createParticleGroupCallback.system = this + createParticleGroupCallback.def = groupDef + createParticleGroupCallback.firstIndex = firstIndex + diagram.getNodes(createParticleGroupCallback) + } + if (groupDef.groupFlags and ParticleGroupType.b2_solidParticleGroup != 0) { + computeDepthForGroup(group) + } + + return group + } + + fun joinParticleGroups(groupA: ParticleGroup, groupB: ParticleGroup) { + assert(groupA != groupB) + RotateBuffer(groupB.m_firstIndex, groupB.m_lastIndex, particleCount) + assert(groupB.m_lastIndex == particleCount) + RotateBuffer(groupA.m_firstIndex, groupA.m_lastIndex, groupB.m_firstIndex) + assert(groupA.m_lastIndex == groupB.m_firstIndex) + + var particleFlags = 0 + for (i in groupA.m_firstIndex until groupB.m_lastIndex) { + particleFlags = particleFlags or m_flagsBuffer.data!![i] + } + + updateContacts(true) + if (particleFlags and k_pairFlags != 0) { + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + var a = contact.indexA + var b = contact.indexB + if (a > b) { + val temp = a + a = b + b = temp + } + if (groupA.m_firstIndex <= a && a < groupA.m_lastIndex && groupB.m_firstIndex <= b + && b < groupB.m_lastIndex) { + if (m_pairCount >= m_pairCapacity) { + val oldCapacity = m_pairCapacity + val newCapacity = if (m_pairCount != 0) 2 * m_pairCount else Settings.minParticleBufferCapacity + m_pairBuffer = BufferUtils.reallocateBuffer({ Pair() }, m_pairBuffer, oldCapacity, newCapacity) + m_pairCapacity = newCapacity + } + val pair = m_pairBuffer[m_pairCount] + pair.indexA = a + pair.indexB = b + pair.flags = contact.flags + pair.strength = MathUtils.min(groupA.m_strength, groupB.m_strength) + pair.distance = MathUtils.distance(m_positionBuffer.data!![a], m_positionBuffer.data!![b]) + m_pairCount++ + } + } + } + if (particleFlags and k_triadFlags != 0) { + val diagram = VoronoiDiagram(groupB.m_lastIndex - groupA.m_firstIndex) + for (i in groupA.m_firstIndex until groupB.m_lastIndex) { + if (m_flagsBuffer.data!![i] and ParticleType.b2_zombieParticle == 0) { + diagram.addGenerator(m_positionBuffer.data!![i], i) + } + } + diagram.generate(particleStride / 2) + val callback = JoinParticleGroupsCallback() + callback.system = this + callback.groupA = groupA + callback.groupB = groupB + diagram.getNodes(callback) + } + + for (i in groupB.m_firstIndex until groupB.m_lastIndex) { + particleGroupBuffer!![i] = groupA + } + val groupFlags = groupA.m_groupFlags or groupB.m_groupFlags + groupA.m_groupFlags = groupFlags + groupA.m_lastIndex = groupB.m_lastIndex + groupB.m_firstIndex = groupB.m_lastIndex + destroyParticleGroup(groupB) + + if (groupFlags and ParticleGroupType.b2_solidParticleGroup != 0) { + computeDepthForGroup(groupA) + } + } + + // Only called from solveZombie() or joinParticleGroups(). + internal fun destroyParticleGroup(group: ParticleGroup?) { + assert(particleGroupCount > 0) + assert(group != null) + + if (m_world.particleDestructionListener != null) { + m_world.particleDestructionListener!!.sayGoodbye(group!!) + } + + for (i in group!!.m_firstIndex until group.m_lastIndex) { + particleGroupBuffer!![i] = null + } + + if (group.m_prev != null) { + group.m_prev!!.m_next = group.m_next + } + if (group.m_next != null) { + group.m_next!!.m_prev = group.m_prev + } + if (group == m_groupList) { + m_groupList = group.m_next + } + + --particleGroupCount + } + + fun computeDepthForGroup(group: ParticleGroup) { + for (i in group.m_firstIndex until group.m_lastIndex) { + m_accumulationBuffer!![i] = 0f + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + if (a >= group.m_firstIndex && a < group.m_lastIndex && b >= group.m_firstIndex + && b < group.m_lastIndex) { + val w = contact.weight + m_accumulationBuffer!![a] += w + m_accumulationBuffer!![b] += w + } + } + m_depthBuffer = requestParticleBuffer(m_depthBuffer) + for (i in group.m_firstIndex until group.m_lastIndex) { + val w = m_accumulationBuffer!![i] + m_depthBuffer!![i] = if (w < 0.8f) 0f else Float.MAX_VALUE + } + val interationCount = group.particleCount + for (t in 0 until interationCount) { + var updated = false + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + if (a >= group.m_firstIndex && a < group.m_lastIndex && b >= group.m_firstIndex + && b < group.m_lastIndex) { + val r = 1 - contact.weight + val ap0 = m_depthBuffer!![a] + val bp0 = m_depthBuffer!![b] + val ap1 = bp0 + r + val bp1 = ap0 + r + if (ap0 > ap1) { + m_depthBuffer!![a] = ap1 + updated = true + } + if (bp0 > bp1) { + m_depthBuffer!![b] = bp1 + updated = true + } + } + } + if (!updated) { + break + } + } + for (i in group.m_firstIndex until group.m_lastIndex) { + val p = m_depthBuffer!![i] + if (p < Float.MAX_VALUE) { + m_depthBuffer!![i] *= m_particleDiameter + } else { + m_depthBuffer!![i] = 0f + } + } + } + + fun addContact(a: Int, b: Int) { + assert(a != b) + val pa = m_positionBuffer.data!![a] + val pb = m_positionBuffer.data!![b] + val dx = pb.x - pa.x + val dy = pb.y - pa.y + val d2 = dx * dx + dy * dy + // assert(d2 != 0); + if (d2 < m_squaredDiameter) { + if (m_contactCount >= m_contactCapacity) { + val oldCapacity = m_contactCapacity + val newCapacity = if (m_contactCount != 0) 2 * m_contactCount else Settings.minParticleBufferCapacity + m_contactBuffer = BufferUtils.reallocateBuffer({ ParticleContact() }, m_contactBuffer, oldCapacity, + newCapacity) + m_contactCapacity = newCapacity + } + val invD = if (d2 != 0f) MathUtils.sqrt(1 / d2) else Float.MAX_VALUE + val contact = m_contactBuffer[m_contactCount] + contact.indexA = a + contact.indexB = b + contact.flags = m_flagsBuffer.data!![a] or m_flagsBuffer.data!![b] + contact.weight = 1 - d2 * invD * m_inverseDiameter + contact.normal.x = invD * dx + contact.normal.y = invD * dy + m_contactCount++ + } + } + + fun updateContacts(exceptZombie: Boolean) { + for (p in 0 until m_proxyCount) { + val proxy = m_proxyBuffer[p] + val i = proxy.index + val pos = m_positionBuffer.data!![i] + proxy.tag = computeTag(m_inverseDiameter * pos.x, m_inverseDiameter * pos.y) + } + Arrays_sort(m_proxyBuffer, 0, m_proxyCount) + m_contactCount = 0 + var c_index = 0 + for (i in 0 until m_proxyCount) { + val a = m_proxyBuffer[i] + val rightTag = computeRelativeTag(a.tag, 1, 0) + for (j in i + 1 until m_proxyCount) { + val b = m_proxyBuffer[j] + if (rightTag < b.tag) { + break + } + addContact(a.index, b.index) + } + val bottomLeftTag = computeRelativeTag(a.tag, -1, 1) + while (c_index < m_proxyCount) { + val c = m_proxyBuffer[c_index] + if (bottomLeftTag <= c.tag) { + break + } + c_index++ + } + val bottomRightTag = computeRelativeTag(a.tag, 1, 1) + + for (b_index in c_index until m_proxyCount) { + val b = m_proxyBuffer[b_index] + if (bottomRightTag < b.tag) { + break + } + addContact(a.index, b.index) + } + } + if (exceptZombie) { + var j = m_contactCount + var i = 0 + while (i < j) { + if (m_contactBuffer[i].flags and ParticleType.b2_zombieParticle != 0) { + --j + val temp = m_contactBuffer[j] + m_contactBuffer[j] = m_contactBuffer[i] + m_contactBuffer[i] = temp + --i + } + i++ + } + m_contactCount = j + } + } + + fun updateBodyContacts() { + val aabb = temp + aabb.lowerBound.x = Float.MAX_VALUE + aabb.lowerBound.y = Float.MAX_VALUE + aabb.upperBound.x = -Float.MAX_VALUE + aabb.upperBound.y = -Float.MAX_VALUE + for (i in 0 until particleCount) { + val p = m_positionBuffer.data!![i] + Vec2.minToOut(aabb.lowerBound, p, aabb.lowerBound) + Vec2.maxToOut(aabb.upperBound, p, aabb.upperBound) + } + aabb.lowerBound.x -= m_particleDiameter + aabb.lowerBound.y -= m_particleDiameter + aabb.upperBound.x += m_particleDiameter + aabb.upperBound.y += m_particleDiameter + m_bodyContactCount = 0 + + ubccallback.system = this + m_world.queryAABB(ubccallback, aabb) + } + + fun solveCollision(step: TimeStep) { + val aabb = temp + val lowerBound = aabb.lowerBound + val upperBound = aabb.upperBound + lowerBound.x = Float.MAX_VALUE + lowerBound.y = Float.MAX_VALUE + upperBound.x = -Float.MAX_VALUE + upperBound.y = -Float.MAX_VALUE + for (i in 0 until particleCount) { + val v = m_velocityBuffer.data!![i] + val p1 = m_positionBuffer.data!![i] + val p1x = p1.x + val p1y = p1.y + val p2x = p1x + step.dt * v.x + val p2y = p1y + step.dt * v.y + val bx = if (p1x < p2x) p1x else p2x + val by = if (p1y < p2y) p1y else p2y + lowerBound.x = if (lowerBound.x < bx) lowerBound.x else bx + lowerBound.y = if (lowerBound.y < by) lowerBound.y else by + val b1x = if (p1x > p2x) p1x else p2x + val b1y = if (p1y > p2y) p1y else p2y + upperBound.x = if (upperBound.x > b1x) upperBound.x else b1x + upperBound.y = if (upperBound.y > b1y) upperBound.y else b1y + } + sccallback.step = step + sccallback.system = this + m_world.queryAABB(sccallback, aabb) + } + + fun solve(step: TimeStep) { + ++m_timestamp + if (particleCount == 0) { + return + } + m_allParticleFlags = 0 + for (i in 0 until particleCount) { + m_allParticleFlags = m_allParticleFlags or m_flagsBuffer.data!![i] + } + if (m_allParticleFlags and ParticleType.b2_zombieParticle != 0) { + solveZombie() + } + if (particleCount == 0) { + return + } + m_allGroupFlags = 0 + var group = m_groupList + while (group != null) { + m_allGroupFlags = m_allGroupFlags or group.m_groupFlags + group = group.getNext() + } + val gravityx = step.dt * particleGravityScale * m_world.gravity.x + val gravityy = step.dt * particleGravityScale * m_world.gravity.y + val criticalVelocytySquared = getCriticalVelocitySquared(step) + for (i in 0 until particleCount) { + val v = m_velocityBuffer.data!![i] + v.x += gravityx + v.y += gravityy + val v2 = v.x * v.x + v.y * v.y + if (v2 > criticalVelocytySquared) { + val a = if (v2 == 0f) Float.MAX_VALUE else MathUtils.sqrt(criticalVelocytySquared / v2) + v.x *= a + v.y *= a + } + } + solveCollision(step) + if (m_allGroupFlags and ParticleGroupType.b2_rigidParticleGroup != 0) { + solveRigid(step) + } + if (m_allParticleFlags and ParticleType.b2_wallParticle != 0) { + solveWall(step) + } + for (i in 0 until particleCount) { + val pos = m_positionBuffer.data!![i] + val vel = m_velocityBuffer.data!![i] + pos.x += step.dt * vel.x + pos.y += step.dt * vel.y + } + updateBodyContacts() + updateContacts(false) + if (m_allParticleFlags and ParticleType.b2_viscousParticle != 0) { + solveViscous(step) + } + if (m_allParticleFlags and ParticleType.b2_powderParticle != 0) { + solvePowder(step) + } + if (m_allParticleFlags and ParticleType.b2_tensileParticle != 0) { + solveTensile(step) + } + if (m_allParticleFlags and ParticleType.b2_elasticParticle != 0) { + solveElastic(step) + } + if (m_allParticleFlags and ParticleType.b2_springParticle != 0) { + solveSpring(step) + } + if (m_allGroupFlags and ParticleGroupType.b2_solidParticleGroup != 0) { + solveSolid(step) + } + if (m_allParticleFlags and ParticleType.b2_colorMixingParticle != 0) { + solveColorMixing(step) + } + solvePressure(step) + solveDamping(step) + } + + internal fun solvePressure(step: TimeStep) { + // calculates the sum of contact-weights for each particle + // that means dimensionless density + for (i in 0 until particleCount) { + m_accumulationBuffer[i] = 0f + } + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + val a = contact.index + val w = contact.weight + m_accumulationBuffer[a] += w + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + m_accumulationBuffer[a] += w + m_accumulationBuffer[b] += w + } + // ignores powder particles + if (m_allParticleFlags and k_noPressureFlags != 0) { + for (i in 0 until particleCount) { + if (m_flagsBuffer.data!![i] and k_noPressureFlags != 0) { + m_accumulationBuffer[i] = 0f + } + } + } + // calculates pressure as a linear function of density + val pressurePerWeight = m_pressureStrength * getCriticalPressure(step) + for (i in 0 until particleCount) { + val w = m_accumulationBuffer!![i] + val h = pressurePerWeight * MathUtils.max(0.0f, MathUtils.min(w, Settings.maxParticleWeight) - Settings.minParticleWeight) + m_accumulationBuffer[i] = h + } + // applies pressure between each particles in contact + val velocityPerPressure = step.dt / (m_density * m_particleDiameter) + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + val a = contact.index + val b = contact.body + val w = contact.weight + val m = contact.mass + val n = contact.normal + val p = m_positionBuffer.data!![a] + val h = m_accumulationBuffer!![a] + pressurePerWeight * w + val f = tempVec + val coef = velocityPerPressure * w * m * h + f.x = coef * n.x + f.y = coef * n.y + val velData = m_velocityBuffer.data!![a] + val particleInvMass = particleInvMass + velData.x -= particleInvMass * f.x + velData.y -= particleInvMass * f.y + b!!.applyLinearImpulse(f, p, true) + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + val n = contact.normal + val h = m_accumulationBuffer!![a] + m_accumulationBuffer!![b] + val fx = velocityPerPressure * w * h * n.x + val fy = velocityPerPressure * w * h * n.y + val velDataA = m_velocityBuffer.data!![a] + val velDataB = m_velocityBuffer.data!![b] + velDataA.x -= fx + velDataA.y -= fy + velDataB.x += fx + velDataB.y += fy + } + } + + internal fun solveDamping(step: TimeStep) { + // reduces normal velocity of each contact + val damping = particleDamping + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + val a = contact.index + val b = contact.body + val w = contact.weight + val m = contact.mass + val n = contact.normal + val p = m_positionBuffer.data!![a] + val tempX = p.x - b!!.sweep.c.x + val tempY = p.y - b!!.sweep.c.y + val velA = m_velocityBuffer.data!![a] + // getLinearVelocityFromWorldPointToOut, with -= velA + val vx = -b!!._angularVelocity * tempY + b!!._linearVelocity.x - velA.x + val vy = b!!._angularVelocity * tempX + b!!._linearVelocity.y - velA.y + // done + val vn = vx * n.x + vy * n.y + if (vn < 0) { + val f = tempVec + f.x = damping * w * m * vn * n.x + f.y = damping * w * m * vn * n.y + val invMass = particleInvMass + velA.x += invMass * f.x + velA.y += invMass * f.y + f.x = -f.x + f.y = -f.y + b!!.applyLinearImpulse(f, p, true) + } + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + val n = contact.normal + val velA = m_velocityBuffer.data!![a] + val velB = m_velocityBuffer.data!![b] + val vx = velB.x - velA.x + val vy = velB.y - velA.y + val vn = vx * n.x + vy * n.y + if (vn < 0) { + val fx = damping * w * vn * n.x + val fy = damping * w * vn * n.y + velA.x += fx + velA.y += fy + velB.x -= fx + velB.y -= fy + } + } + } + + fun solveWall(step: TimeStep) { + for (i in 0 until particleCount) { + if (m_flagsBuffer.data!![i] and ParticleType.b2_wallParticle != 0) { + val r = m_velocityBuffer.data!![i] + r.x = 0.0f + r.y = 0.0f + } + } + } + + internal fun solveRigid(step: TimeStep) { + var group = m_groupList + while (group != null) { + if (group.m_groupFlags and ParticleGroupType.b2_rigidParticleGroup != 0) { + group.updateStatistics() + val temp = tempVec + val cross = tempVec2 + val rotation = tempRot + rotation.setRadians(step.dt * group.m_angularVelocity) + Rot.mulToOutUnsafe(rotation, group.m_center, cross) + temp.set(group.m_linearVelocity).mulLocal(step.dt).addLocal(group.m_center).subLocal(cross) + tempXf.p.set(temp) + tempXf.q.set(rotation) + Transform.mulToOut(tempXf, group.m_transform, group.m_transform) + val velocityTransform = tempXf2 + velocityTransform.p.x = step.inv_dt * tempXf.p.x + velocityTransform.p.y = step.inv_dt * tempXf.p.y + velocityTransform.q.s = step.inv_dt * tempXf.q.s + velocityTransform.q.c = step.inv_dt * (tempXf.q.c - 1) + for (i in group.m_firstIndex until group.m_lastIndex) { + Transform.mulToOutUnsafe(velocityTransform, m_positionBuffer.data!![i], + m_velocityBuffer.data!![i]) + } + } + group = group.getNext() + } + } + + internal fun solveElastic(step: TimeStep) { + val elasticStrength = step.inv_dt * m_elasticStrength + for (k in 0 until m_triadCount) { + val triad = m_triadBuffer[k] + if (triad.flags and ParticleType.b2_elasticParticle != 0) { + val a = triad.indexA + val b = triad.indexB + val c = triad.indexC + val oa = triad.pa + val ob = triad.pb + val oc = triad.pc + val pa = m_positionBuffer.data!![a] + val pb = m_positionBuffer.data!![b] + val pc = m_positionBuffer.data!![c] + val px = 1f / 3 * (pa.x + pb.x + pc.x) + val py = 1f / 3 * (pa.y + pb.y + pc.y) + var rs = Vec2.cross(oa, pa) + Vec2.cross(ob, pb) + Vec2.cross(oc, pc) + var rc = Vec2.dot(oa, pa) + Vec2.dot(ob, pb) + Vec2.dot(oc, pc) + val r2 = rs * rs + rc * rc + val invR = if (r2 == 0f) Float.MAX_VALUE else MathUtils.sqrt(1f / r2) + rs *= invR + rc *= invR + val strength = elasticStrength * triad.strength + val roax = rc * oa.x - rs * oa.y + val roay = rs * oa.x + rc * oa.y + val robx = rc * ob.x - rs * ob.y + val roby = rs * ob.x + rc * ob.y + val rocx = rc * oc.x - rs * oc.y + val rocy = rs * oc.x + rc * oc.y + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + val vc = m_velocityBuffer.data!![c] + va.x += strength * (roax - (pa.x - px)) + va.y += strength * (roay - (pa.y - py)) + vb.x += strength * (robx - (pb.x - px)) + vb.y += strength * (roby - (pb.y - py)) + vc.x += strength * (rocx - (pc.x - px)) + vc.y += strength * (rocy - (pc.y - py)) + } + } + } + + internal fun solveSpring(step: TimeStep) { + val springStrength = step.inv_dt * m_springStrength + for (k in 0 until m_pairCount) { + val pair = m_pairBuffer[k] + if (pair.flags and ParticleType.b2_springParticle != 0) { + val a = pair.indexA + val b = pair.indexB + val pa = m_positionBuffer.data!![a] + val pb = m_positionBuffer.data!![b] + val dx = pb.x - pa.x + val dy = pb.y - pa.y + val r0 = pair.distance + var r1 = MathUtils.sqrt(dx * dx + dy * dy) + if (r1 == 0f) r1 = Float.MAX_VALUE + val strength = springStrength * pair.strength + val fx = strength * (r0 - r1) / r1 * dx + val fy = strength * (r0 - r1) / r1 * dy + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + va.x -= fx + va.y -= fy + vb.x += fx + vb.y += fy + } + } + } + + internal fun solveTensile(step: TimeStep) { + m_accumulation2Buffer = requestParticleBuffer({ Vec2() }, m_accumulation2Buffer) + for (i in 0 until particleCount) { + m_accumulationBuffer[i] = 0f + m_accumulation2Buffer!![i].setZero() + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + if (contact.flags and ParticleType.b2_tensileParticle != 0) { + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + val n = contact.normal + m_accumulationBuffer[a] += w + m_accumulationBuffer[b] += w + val a2A = m_accumulation2Buffer!![a] + val a2B = m_accumulation2Buffer!![b] + val inter = (1 - w) * w + a2A.x -= inter * n.x + a2A.y -= inter * n.y + a2B.x += inter * n.x + a2B.y += inter * n.y + } + } + val strengthA = m_surfaceTensionStrengthA * getCriticalVelocity(step) + val strengthB = m_surfaceTensionStrengthB * getCriticalVelocity(step) + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + if (contact.flags and ParticleType.b2_tensileParticle != 0) { + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + val n = contact.normal + val a2A = m_accumulation2Buffer!![a] + val a2B = m_accumulation2Buffer!![b] + val h = m_accumulationBuffer!![a] + m_accumulationBuffer!![b] + val sx = a2B.x - a2A.x + val sy = a2B.y - a2A.y + val fn = (strengthA * (h - 2) + strengthB * (sx * n.x + sy * n.y)) * w + val fx = fn * n.x + val fy = fn * n.y + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + va.x -= fx + va.y -= fy + vb.x += fx + vb.y += fy + } + } + } + + internal fun solveViscous(step: TimeStep) { + val viscousStrength = m_viscousStrength + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + val a = contact.index + if (m_flagsBuffer.data!![a] and ParticleType.b2_viscousParticle != 0) { + val b = contact.body + val w = contact.weight + val m = contact.mass + val p = m_positionBuffer.data!![a] + val va = m_velocityBuffer.data!![a] + val tempX = p.x - b!!.sweep.c.x + val tempY = p.y - b!!.sweep.c.y + val vx = -b!!._angularVelocity * tempY + b!!._linearVelocity.x - va.x + val vy = b!!._angularVelocity * tempX + b!!._linearVelocity.y - va.y + val f = tempVec + val pInvMass = particleInvMass + f.x = viscousStrength * m * w * vx + f.y = viscousStrength * m * w * vy + va.x += pInvMass * f.x + va.y += pInvMass * f.y + f.x = -f.x + f.y = -f.y + b!!.applyLinearImpulse(f, p, true) + } + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + if (contact.flags and ParticleType.b2_viscousParticle != 0) { + val a = contact.indexA + val b = contact.indexB + val w = contact.weight + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + val vx = vb.x - va.x + val vy = vb.y - va.y + val fx = viscousStrength * w * vx + val fy = viscousStrength * w * vy + va.x += fx + va.y += fy + vb.x -= fx + vb.y -= fy + } + } + } + + internal fun solvePowder(step: TimeStep) { + val powderStrength = m_powderStrength * getCriticalVelocity(step) + val minWeight = 1.0f - Settings.particleStride + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + val a = contact.index + if (m_flagsBuffer.data!![a] and ParticleType.b2_powderParticle != 0) { + val w = contact.weight + if (w > minWeight) { + val b = contact.body + val m = contact.mass + val p = m_positionBuffer.data!![a] + val n = contact.normal + val f = tempVec + val va = m_velocityBuffer.data!![a] + val inter = powderStrength * m * (w - minWeight) + val pInvMass = particleInvMass + f.x = inter * n.x + f.y = inter * n.y + va.x -= pInvMass * f.x + va.y -= pInvMass * f.y + b!!.applyLinearImpulse(f, p, true) + } + } + } + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + if (contact.flags and ParticleType.b2_powderParticle != 0) { + val w = contact.weight + if (w > minWeight) { + val a = contact.indexA + val b = contact.indexB + val n = contact.normal + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + val inter = powderStrength * (w - minWeight) + val fx = inter * n.x + val fy = inter * n.y + va.x -= fx + va.y -= fy + vb.x += fx + vb.y += fy + } + } + } + } + + internal fun solveSolid(step: TimeStep) { + // applies extra repulsive force from solid particle groups + m_depthBuffer = requestParticleBuffer(m_depthBuffer) + val ejectionStrength = step.inv_dt * m_ejectionStrength + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + if (particleGroupBuffer!![a] != particleGroupBuffer!![b]) { + val w = contact.weight + val n = contact.normal + val h = m_depthBuffer!![a] + m_depthBuffer!![b] + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + val inter = ejectionStrength * h * w + val fx = inter * n.x + val fy = inter * n.y + va.x -= fx + va.y -= fy + vb.x += fx + vb.y += fy + } + } + } + + internal fun solveColorMixing(step: TimeStep) { + // mixes color between contacting particles + m_colorBuffer.data = requestParticleBuffer({ ParticleColor() }, m_colorBuffer.data) + val colorMixing256 = (256 * m_colorMixingStrength).toInt() + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + if (m_flagsBuffer.data!![a] and m_flagsBuffer.data!![b] and ParticleType.b2_colorMixingParticle != 0) { + val colorA = m_colorBuffer.data!![a]!! + val colorB = m_colorBuffer.data!![b]!! + val dr = (colorMixing256 * ((colorB.r.toInt() and 0xFF) - (colorA.r.toInt() and 0xFF))) ushr 8 + val dg = (colorMixing256 * ((colorB.g.toInt() and 0xFF) - (colorA.g.toInt() and 0xFF))) ushr 8 + val db = (colorMixing256 * ((colorB.b.toInt() and 0xFF) - (colorA.b.toInt() and 0xFF))) ushr 8 + val da = (colorMixing256 * ((colorB.a.toInt() and 0xFF) - (colorA.a.toInt() and 0xFF))) ushr 8 + colorA.r = (colorA.r.toInt() + dr).toByte() + colorA.g = (colorA.g.toInt() + dg).toByte() + colorA.b = (colorA.b.toInt() + db).toByte() + colorA.a = (colorA.a.toInt() + da).toByte() + colorB.r = (colorB.r.toInt() - dr).toByte() + colorB.g = (colorB.g.toInt() - dg).toByte() + colorB.b = (colorB.b.toInt() - db).toByte() + colorB.a = (colorB.a.toInt() - da).toByte() + } + } + } + + internal fun solveZombie() { + // removes particles with zombie flag + var newCount = 0 + val newIndices = IntArray(particleCount) + for (i in 0 until particleCount) { + val flags = m_flagsBuffer.data!![i] + if (flags and ParticleType.b2_zombieParticle != 0) { + val destructionListener = m_world.particleDestructionListener + if (flags and ParticleType.b2_destructionListener != 0 && destructionListener != null) { + destructionListener.sayGoodbye(i) + } + newIndices[i] = Settings.invalidParticleIndex + } else { + newIndices[i] = newCount + if (i != newCount) { + m_flagsBuffer!!.data!![newCount] = m_flagsBuffer.data!![i] + m_positionBuffer.data!![newCount].set(m_positionBuffer.data!![i]) + m_velocityBuffer.data!![newCount].set(m_velocityBuffer.data!![i]) + particleGroupBuffer!![newCount] = particleGroupBuffer!![i]!! + if (m_depthBuffer != null) { + m_depthBuffer!![newCount] = m_depthBuffer!![i] + } + if (m_colorBuffer.data != null) { + m_colorBuffer.data!![newCount].set(m_colorBuffer.data!![i]) + } + if (m_userDataBuffer.data != null) { + m_userDataBuffer!!.data!![newCount] = m_userDataBuffer.data!![i] + } + } + newCount++ + } + } + + // update proxies + for (k in 0 until m_proxyCount) { + val proxy = m_proxyBuffer[k] + proxy.index = newIndices[proxy.index] + } + + // Proxy lastProxy = std.remove_if( + // m_proxyBuffer, m_proxyBuffer + m_proxyCount, + // Test.IsProxyInvalid); + // m_proxyCount = (int) (lastProxy - m_proxyBuffer); + var j = m_proxyCount + run { + var i = 0 + while (i < j) { + if (Test.IsProxyInvalid(m_proxyBuffer[i])) { + --j + val temp = m_proxyBuffer[j] + m_proxyBuffer[j] = m_proxyBuffer[i] + m_proxyBuffer[i] = temp + --i + } + i++ + } + } + m_proxyCount = j + + // update contacts + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + contact.indexA = newIndices[contact.indexA] + contact.indexB = newIndices[contact.indexB] + } + // ParticleContact lastContact = std.remove_if( + // m_contactBuffer, m_contactBuffer + m_contactCount, + // Test.IsContactInvalid); + // m_contactCount = (int) (lastContact - m_contactBuffer); + j = m_contactCount + run { + var i = 0 + while (i < j) { + if (Test.IsContactInvalid(m_contactBuffer[i])) { + --j + val temp = m_contactBuffer[j] + m_contactBuffer[j] = m_contactBuffer[i] + m_contactBuffer[i] = temp + --i + } + i++ + } + } + m_contactCount = j + + // update particle-body contacts + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + contact.index = newIndices[contact.index] + } + // ParticleBodyContact lastBodyContact = std.remove_if( + // m_bodyContactBuffer, m_bodyContactBuffer + m_bodyContactCount, + // Test.IsBodyContactInvalid); + // m_bodyContactCount = (int) (lastBodyContact - m_bodyContactBuffer); + j = m_bodyContactCount + run { + var i = 0 + while (i < j) { + if (Test.IsBodyContactInvalid(m_bodyContactBuffer[i])) { + --j + val temp = m_bodyContactBuffer[j] + m_bodyContactBuffer[j] = m_bodyContactBuffer[i] + m_bodyContactBuffer[i] = temp + --i + } + i++ + } + } + m_bodyContactCount = j + + // update pairs + for (k in 0 until m_pairCount) { + val pair = m_pairBuffer[k] + pair.indexA = newIndices[pair.indexA] + pair.indexB = newIndices[pair.indexB] + } + // Pair lastPair = std.remove_if(m_pairBuffer, m_pairBuffer + m_pairCount, Test.IsPairInvalid); + // m_pairCount = (int) (lastPair - m_pairBuffer); + j = m_pairCount + run { + var i = 0 + while (i < j) { + if (Test.IsPairInvalid(m_pairBuffer[i])) { + --j + val temp = m_pairBuffer[j] + m_pairBuffer[j] = m_pairBuffer[i] + m_pairBuffer[i] = temp + --i + } + i++ + } + } + m_pairCount = j + + // update triads + for (k in 0 until m_triadCount) { + val triad = m_triadBuffer[k] + triad.indexA = newIndices[triad.indexA] + triad.indexB = newIndices[triad.indexB] + triad.indexC = newIndices[triad.indexC] + } + // Triad lastTriad = + // std.remove_if(m_triadBuffer, m_triadBuffer + m_triadCount, Test.isTriadInvalid); + // m_triadCount = (int) (lastTriad - m_triadBuffer); + j = m_triadCount + run { + var i = 0 + while (i < j) { + if (Test.IsTriadInvalid(m_triadBuffer[i])) { + --j + val temp = m_triadBuffer[j] + m_triadBuffer[j] = m_triadBuffer[i] + m_triadBuffer[i] = temp + --i + } + i++ + } + } + m_triadCount = j + + // update groups + run { + var group = m_groupList + while (group != null) { + var firstIndex = newCount + var lastIndex = 0 + var modified = false + for (i in group!!.m_firstIndex until group!!.m_lastIndex) { + j = newIndices[i] + if (j >= 0) { + firstIndex = MathUtils.min(firstIndex, j) + lastIndex = MathUtils.max(lastIndex, j + 1) + } else { + modified = true + } + } + if (firstIndex < lastIndex) { + group!!.m_firstIndex = firstIndex + group!!.m_lastIndex = lastIndex + if (modified) { + if (group!!.m_groupFlags and ParticleGroupType.b2_rigidParticleGroup != 0) { + group!!.m_toBeSplit = true + } + } + } else { + group!!.m_firstIndex = 0 + group!!.m_lastIndex = 0 + if (group!!.m_destroyAutomatically) { + group!!.m_toBeDestroyed = true + } + } + group = group!!.getNext() + } + } + + // update particle count + particleCount = newCount + // m_world.m_stackAllocator.Free(newIndices); + + // destroy bodies with no particles + var group = m_groupList + while (group != null) { + val next = group!!.getNext() + if (group!!.m_toBeDestroyed) { + destroyParticleGroup(group) + } else if (group!!.m_toBeSplit) { + // TODO: split the group + } + group = next + } + } + + private class NewIndices { + internal var start: Int = 0 + internal var mid: Int = 0 + internal var end: Int = 0 + + internal fun getIndex(i: Int): Int { + return if (i < start) { + i + } else if (i < mid) { + i + end - mid + } else if (i < end) { + i + start - mid + } else { + i + } + } + } + + + internal fun RotateBuffer(start: Int, mid: Int, end: Int) { + // move the particles assigned to the given group toward the end of array + if (start == mid || mid == end) { + return + } + newIndices.start = start + newIndices.mid = mid + newIndices.end = end + + BufferUtils.rotate(m_flagsBuffer.data!!, start, mid, end) + BufferUtils.rotate(m_positionBuffer.data!!, start, mid, end) + BufferUtils.rotate(m_velocityBuffer.data!!, start, mid, end) + BufferUtils.rotate(particleGroupBuffer!! as Array, start, mid, end) + if (m_depthBuffer != null) { + BufferUtils.rotate(m_depthBuffer!!, start, mid, end) + } + if (m_colorBuffer.data != null) { + BufferUtils.rotate(m_colorBuffer.data!!, start, mid, end) + } + if (m_userDataBuffer.data != null) { + BufferUtils.rotate(m_userDataBuffer.data!!, start, mid, end) + } + + // update proxies + for (k in 0 until m_proxyCount) { + val proxy = m_proxyBuffer[k] + proxy.index = newIndices.getIndex(proxy.index) + } + + // update contacts + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + contact.indexA = newIndices.getIndex(contact.indexA) + contact.indexB = newIndices.getIndex(contact.indexB) + } + + // update particle-body contacts + for (k in 0 until m_bodyContactCount) { + val contact = m_bodyContactBuffer[k] + contact.index = newIndices.getIndex(contact.index) + } + + // update pairs + for (k in 0 until m_pairCount) { + val pair = m_pairBuffer[k] + pair.indexA = newIndices.getIndex(pair.indexA) + pair.indexB = newIndices.getIndex(pair.indexB) + } + + // update triads + for (k in 0 until m_triadCount) { + val triad = m_triadBuffer[k] + triad.indexA = newIndices.getIndex(triad.indexA) + triad.indexB = newIndices.getIndex(triad.indexB) + triad.indexC = newIndices.getIndex(triad.indexC) + } + + // update groups + var group = m_groupList + while (group != null) { + group.m_firstIndex = newIndices.getIndex(group.m_firstIndex) + group.m_lastIndex = newIndices.getIndex(group.m_lastIndex - 1) + 1 + group = group.getNext() + } + } + + internal fun getCriticalVelocity(step: TimeStep): Float { + return m_particleDiameter * step.inv_dt + } + + internal fun getCriticalVelocitySquared(step: TimeStep): Float { + val velocity = getCriticalVelocity(step) + return velocity * velocity + } + + internal fun getCriticalPressure(step: TimeStep): Float { + return m_density * getCriticalVelocitySquared(step) + } + + internal fun setParticleBuffer(buffer: ParticleBufferInt, newData: IntArray?, newCapacity: Int) { + assert(newData != null && newCapacity != 0 || newData == null && newCapacity == 0) + if (buffer.userSuppliedCapacity != 0) { + // m_world.m_blockAllocator.Free(buffer.data, sizeof(T) * m_internalAllocatedCapacity); + } + buffer.data = newData + buffer.userSuppliedCapacity = newCapacity + } + + internal fun setParticleBuffer(buffer: ParticleBuffer, newData: Array?, newCapacity: Int) { + assert(newData != null && newCapacity != 0 || newData == null && newCapacity == 0) + if (buffer.userSuppliedCapacity != 0) { + // m_world.m_blockAllocator.Free(buffer.data, sizeof(T) * m_internalAllocatedCapacity); + } + buffer.data = newData + buffer.userSuppliedCapacity = newCapacity + } + + fun setParticleFlagsBuffer(buffer: IntArray, capacity: Int) { + setParticleBuffer(m_flagsBuffer, buffer, capacity) + } + + fun setParticlePositionBuffer(buffer: Array, capacity: Int) { + setParticleBuffer(m_positionBuffer, buffer, capacity) + } + + fun setParticleVelocityBuffer(buffer: Array, capacity: Int) { + setParticleBuffer(m_velocityBuffer, buffer, capacity) + } + + fun setParticleColorBuffer(buffer: Array, capacity: Int) { + setParticleBuffer(m_colorBuffer, buffer, capacity) + } + + fun getParticleGroupList(): Array? { + return particleGroupBuffer!! + } + + fun setParticleUserDataBuffer(buffer: Array, capacity: Int) { + setParticleBuffer(m_userDataBuffer, buffer, capacity) + } + + fun queryAABB(callback: ParticleQueryCallback, aabb: AABB) { + if (m_proxyCount == 0) { + return + } + + val lowerBoundX = aabb.lowerBound.x + val lowerBoundY = aabb.lowerBound.y + val upperBoundX = aabb.upperBound.x + val upperBoundY = aabb.upperBound.y + val firstProxy = lowerBound(m_proxyBuffer, m_proxyCount, + computeTag(m_inverseDiameter * lowerBoundX, m_inverseDiameter * lowerBoundY)) + val lastProxy = upperBound(m_proxyBuffer, m_proxyCount, + computeTag(m_inverseDiameter * upperBoundX, m_inverseDiameter * upperBoundY)) + for (proxy in firstProxy until lastProxy) { + val i = m_proxyBuffer[proxy].index + val p = m_positionBuffer.data!![i] + if (lowerBoundX < p.x && p.x < upperBoundX && lowerBoundY < p.y && p.y < upperBoundY) { + if (!callback.reportParticle(i)) { + break + } + } + } + } + + /** + * @param callback + * @param point1 + * @param point2 + */ + fun raycast(callback: ParticleRaycastCallback, point1: Vec2, point2: Vec2) { + if (m_proxyCount == 0) { + return + } + val firstProxy = lowerBound( + m_proxyBuffer, + m_proxyCount, + computeTag(m_inverseDiameter * MathUtils.min(point1.x, point2.x) - 1, m_inverseDiameter * MathUtils.min(point1.y, point2.y) - 1)) + val lastProxy = upperBound( + m_proxyBuffer, + m_proxyCount, + computeTag(m_inverseDiameter * MathUtils.max(point1.x, point2.x) + 1, m_inverseDiameter * MathUtils.max(point1.y, point2.y) + 1)) + var fraction = 1f + // solving the following equation: + // ((1-t)*point1+t*point2-position)^2=diameter^2 + // where t is a potential fraction + val vx = point2.x - point1.x + val vy = point2.y - point1.y + var v2 = vx * vx + vy * vy + if (v2 == 0f) v2 = Float.MAX_VALUE + for (proxy in firstProxy until lastProxy) { + val i = m_proxyBuffer[proxy].index + val posI = m_positionBuffer.data!![i] + val px = point1.x - posI.x + val py = point1.y - posI.y + val pv = px * vx + py * vy + val p2 = px * px + py * py + val determinant = pv * pv - v2 * (p2 - m_squaredDiameter) + if (determinant >= 0) { + val sqrtDeterminant = MathUtils.sqrt(determinant) + // find a solution between 0 and fraction + var t = (-pv - sqrtDeterminant) / v2 + if (t > fraction) { + continue + } + if (t < 0) { + t = (-pv + sqrtDeterminant) / v2 + if (t < 0 || t > fraction) { + continue + } + } + val n = tempVec + tempVec.x = px + t * vx + tempVec.y = py + t * vy + n.normalize() + val point = tempVec2 + point.x = point1.x + t * vx + point.y = point1.y + t * vy + val f = callback.reportParticle(i, point, n, t) + fraction = MathUtils.min(fraction, f) + if (fraction <= 0) { + break + } + } + } + } + + fun computeParticleCollisionEnergy(): Float { + var sum_v2 = 0f + for (k in 0 until m_contactCount) { + val contact = m_contactBuffer[k] + val a = contact.indexA + val b = contact.indexB + val n = contact.normal + val va = m_velocityBuffer.data!![a] + val vb = m_velocityBuffer.data!![b] + val vx = vb.x - va.x + val vy = vb.y - va.y + val vn = vx * n.x + vy * n.y + if (vn < 0) { + sum_v2 += vn * vn + } + } + return 0.5f * particleMass * sum_v2 + } + + internal fun requestParticleBuffer(newInstance: () -> T, buffer: Array?): Array { + var buffer = buffer + if (buffer == null) { + buffer = Array(m_internalAllocatedCapacity) { newInstance() } as Array + } + return buffer + } + + internal fun requestParticleBuffer(buffer: FloatArray?): FloatArray { + var buffer = buffer + if (buffer == null) { + buffer = FloatArray(m_internalAllocatedCapacity) + } + return buffer + } + + class ParticleBuffer(internal val dataClass: () -> T) { + var data: Array? = null + internal var userSuppliedCapacity: Int = 0 + } + + internal class ParticleBufferInt { + var data: IntArray? = null + var userSuppliedCapacity: Int = 0 + } + + /** Used for detecting particle contacts */ + class Proxy : Comparable { + internal var index: Int = 0 + internal var tag: Long = 0 + + override fun compareTo(o: Proxy): Int { + return if (tag - o.tag < 0) -1 else if (o.tag == tag) 0 else 1 + } + + override fun equals(obj: Any?): Boolean { + if (this === obj) return true + if (obj == null) return false + if (this::class != obj::class) return false + val other = obj as Proxy? + return tag == other!!.tag + } + } + + /** Connection between two particles */ + class Pair { + internal var indexA: Int = 0 + internal var indexB: Int = 0 + internal var flags: Int = 0 + internal var strength: Float = 0.toFloat() + internal var distance: Float = 0.toFloat() + } + + /** Connection between three particles */ + class Triad { + internal var indexA: Int = 0 + internal var indexB: Int = 0 + internal var indexC: Int = 0 + internal var flags: Int = 0 + internal var strength: Float = 0.toFloat() + internal val pa = Vec2() + internal val pb = Vec2() + internal val pc = Vec2() + internal var ka: Float = 0.toFloat() + internal var kb: Float = 0.toFloat() + internal var kc: Float = 0.toFloat() + internal var s: Float = 0.toFloat() + } + + // Callback used with VoronoiDiagram. + internal class CreateParticleGroupCallback : VoronoiDiagramCallback { + + var system: ParticleSystem? = null + var def: ParticleGroupDef? = null // pointer + var firstIndex: Int = 0 + override fun callback(a: Int, b: Int, c: Int) { + val pa = system!!.m_positionBuffer.data!![a] + val pb = system!!.m_positionBuffer.data!![b] + val pc = system!!.m_positionBuffer.data!![c] + val dabx = pa.x - pb.x + val daby = pa.y - pb.y + val dbcx = pb.x - pc.x + val dbcy = pb.y - pc.y + val dcax = pc.x - pa.x + val dcay = pc.y - pa.y + val maxDistanceSquared = Settings.maxTriadDistanceSquared * system!!.m_squaredDiameter + if (dabx * dabx + daby * daby < maxDistanceSquared + && dbcx * dbcx + dbcy * dbcy < maxDistanceSquared + && dcax * dcax + dcay * dcay < maxDistanceSquared) { + if (system!!.m_triadCount >= system!!.m_triadCapacity) { + val oldCapacity = system!!.m_triadCapacity + val newCapacity = if (system!!.m_triadCount != 0) + 2 * system!!.m_triadCount + else + Settings.minParticleBufferCapacity + system!!.m_triadBuffer = BufferUtils.reallocateBuffer({ Triad() }, system!!.m_triadBuffer, oldCapacity, + newCapacity) + system!!.m_triadCapacity = newCapacity + } + val triad = system!!.m_triadBuffer[system!!.m_triadCount] + triad.indexA = a + triad.indexB = b + triad.indexC = c + triad.flags = (system!!.m_flagsBuffer.data!![a] or system!!.m_flagsBuffer.data!![b] + or system!!.m_flagsBuffer.data!![c]) + triad.strength = def!!.strength + val midPointx = 1.toFloat() / 3 * (pa.x + pb.x + pc.x) + val midPointy = 1.toFloat() / 3 * (pa.y + pb.y + pc.y) + triad.pa.x = pa.x - midPointx + triad.pa.y = pa.y - midPointy + triad.pb.x = pb.x - midPointx + triad.pb.y = pb.y - midPointy + triad.pc.x = pc.x - midPointx + triad.pc.y = pc.y - midPointy + triad.ka = -(dcax * dabx + dcay * daby) + triad.kb = -(dabx * dbcx + daby * dbcy) + triad.kc = -(dbcx * dcax + dbcy * dcay) + triad.s = Vec2.cross(pa, pb) + Vec2.cross(pb, pc) + Vec2.cross(pc, pa) + system!!.m_triadCount++ + } + } + } + + // Callback used with VoronoiDiagram. + internal class JoinParticleGroupsCallback : VoronoiDiagramCallback { + + var system: ParticleSystem? = null + var groupA: ParticleGroup? = null + var groupB: ParticleGroup? = null + override fun callback(a: Int, b: Int, c: Int) { + // Create a triad if it will contain particles from both groups. + val countA = ((if (a < groupB!!.m_firstIndex) 1 else 0) + (if (b < groupB!!.m_firstIndex) 1 else 0) + + if (c < groupB!!.m_firstIndex) 1 else 0) + if (countA > 0 && countA < 3) { + val af = system!!.m_flagsBuffer.data!![a] + val bf = system!!.m_flagsBuffer.data!![b] + val cf = system!!.m_flagsBuffer.data!![c] + if (af and bf and cf and k_triadFlags != 0) { + val pa = system!!.m_positionBuffer.data!![a] + val pb = system!!.m_positionBuffer.data!![b] + val pc = system!!.m_positionBuffer.data!![c] + val dabx = pa.x - pb.x + val daby = pa.y - pb.y + val dbcx = pb.x - pc.x + val dbcy = pb.y - pc.y + val dcax = pc.x - pa.x + val dcay = pc.y - pa.y + val maxDistanceSquared = Settings.maxTriadDistanceSquared * system!!.m_squaredDiameter + if (dabx * dabx + daby * daby < maxDistanceSquared + && dbcx * dbcx + dbcy * dbcy < maxDistanceSquared + && dcax * dcax + dcay * dcay < maxDistanceSquared) { + if (system!!.m_triadCount >= system!!.m_triadCapacity) { + val oldCapacity = system!!.m_triadCapacity + val newCapacity = if (system!!.m_triadCount != 0) + 2 * system!!.m_triadCount + else + Settings.minParticleBufferCapacity + system!!.m_triadBuffer = BufferUtils.reallocateBuffer({ Triad() }, system!!.m_triadBuffer, oldCapacity, + newCapacity) + system!!.m_triadCapacity = newCapacity + } + val triad = system!!.m_triadBuffer[system!!.m_triadCount] + triad.indexA = a + triad.indexB = b + triad.indexC = c + triad.flags = af or bf or cf + triad.strength = MathUtils.min(groupA!!.m_strength, groupB!!.m_strength) + val midPointx = 1.toFloat() / 3 * (pa.x + pb.x + pc.x) + val midPointy = 1.toFloat() / 3 * (pa.y + pb.y + pc.y) + triad.pa.x = pa.x - midPointx + triad.pa.y = pa.y - midPointy + triad.pb.x = pb.x - midPointx + triad.pb.y = pb.y - midPointy + triad.pc.x = pc.x - midPointx + triad.pc.y = pc.y - midPointy + triad.ka = -(dcax * dabx + dcay * daby) + triad.kb = -(dabx * dbcx + daby * dbcy) + triad.kc = -(dbcx * dcax + dbcy * dcay) + triad.s = Vec2.cross(pa, pb) + Vec2.cross(pb, pc) + Vec2.cross(pc, pa) + system!!.m_triadCount++ + } + } + } + } + } + + internal class DestroyParticlesInShapeCallback : ParticleQueryCallback { + lateinit var system: ParticleSystem + lateinit var shape: Shape + lateinit var xf: Transform + var callDestructionListener: Boolean = false + var destroyed: Int = 0 + + fun init(system: ParticleSystem, shape: Shape, xf: Transform, + callDestructionListener: Boolean) { + this.system = system + this.shape = shape + this.xf = xf + this.destroyed = 0 + this.callDestructionListener = callDestructionListener + } + + override fun reportParticle(index: Int): Boolean { + assert(index >= 0 && index < system.particleCount) + if (shape.testPoint(xf, system.m_positionBuffer.data!![index])) { + system.destroyParticle(index, callDestructionListener) + destroyed++ + } + return true + } + }// TODO Auto-generated constructor stub + + internal class UpdateBodyContactsCallback : QueryCallback { + var system: ParticleSystem? = null + + private val tempVec = Vec2() + + override fun reportFixture(fixture: Fixture): Boolean { + if (fixture.isSensor) { + return true + } + val shape = fixture.getShape() + val b = fixture.getBody() + val bp = b!!.worldCenter + val bm = b.getMass() + val bI = b.inertia - bm * b.localCenter.lengthSquared() + val invBm = if (bm > 0) 1f / bm else 0f + val invBI = if (bI > 0) 1f / bI else 0f + val childCount = shape!!.getChildCount() + for (childIndex in 0 until childCount) { + val aabb = fixture.getAABB(childIndex) + val aabblowerBoundx = aabb.lowerBound.x - system!!.m_particleDiameter + val aabblowerBoundy = aabb.lowerBound.y - system!!.m_particleDiameter + val aabbupperBoundx = aabb.upperBound.x + system!!.m_particleDiameter + val aabbupperBoundy = aabb.upperBound.y + system!!.m_particleDiameter + val firstProxy = lowerBound( + system!!.m_proxyBuffer, + system!!.m_proxyCount, + computeTag(system!!.m_inverseDiameter * aabblowerBoundx, system!!.m_inverseDiameter * aabblowerBoundy)) + val lastProxy = upperBound( + system!!.m_proxyBuffer, + system!!.m_proxyCount, + computeTag(system!!.m_inverseDiameter * aabbupperBoundx, system!!.m_inverseDiameter * aabbupperBoundy)) + + for (proxy in firstProxy until lastProxy) { + val a = system!!.m_proxyBuffer[proxy].index + val ap = system!!.m_positionBuffer.data!![a] + if (aabblowerBoundx <= ap.x && ap.x <= aabbupperBoundx && aabblowerBoundy <= ap.y + && ap.y <= aabbupperBoundy) { + val d: Float + val n = tempVec + d = fixture.computeDistance(ap, childIndex, n) + if (d < system!!.m_particleDiameter) { + val invAm = if (system!!.m_flagsBuffer.data!![a] and ParticleType.b2_wallParticle != 0) + 0f + else + system!!.particleInvMass!! + val rpx = ap.x - bp.x + val rpy = ap.y - bp.y + val rpn = rpx * n.y - rpy * n.x + if (system!!.m_bodyContactCount >= system!!.m_bodyContactCapacity) { + val oldCapacity = system!!.m_bodyContactCapacity + val newCapacity = if (system!!.m_bodyContactCount != 0) + 2 * system!!.m_bodyContactCount + else + Settings.minParticleBufferCapacity + system!!.m_bodyContactBuffer = BufferUtils.reallocateBuffer({ ParticleBodyContact() }, + system!!.m_bodyContactBuffer, oldCapacity, newCapacity) + system!!.m_bodyContactCapacity = newCapacity + } + val contact = system!!.m_bodyContactBuffer[system!!.m_bodyContactCount] + contact.index = a + contact.body = b + contact.weight = 1 - d * system!!.m_inverseDiameter + contact.normal.x = -n.x + contact.normal.y = -n.y + contact.mass = 1 / (invAm + invBm + invBI * rpn * rpn) + system!!.m_bodyContactCount++ + } + } + } + } + return true + } + } + + internal class SolveCollisionCallback : QueryCallback { + var system: ParticleSystem? = null + var step: TimeStep? = null + + private val input = RayCastInput() + private val output = RayCastOutput() + private val tempVec = Vec2() + private val tempVec2 = Vec2() + + override fun reportFixture(fixture: Fixture): Boolean { + if (fixture.isSensor) { + return true + } + val shape = fixture.getShape() + val body = fixture.getBody() + val childCount = shape!!.getChildCount() + for (childIndex in 0 until childCount) { + val aabb = fixture.getAABB(childIndex) + val aabblowerBoundx = aabb.lowerBound.x - system!!.m_particleDiameter + val aabblowerBoundy = aabb.lowerBound.y - system!!.m_particleDiameter + val aabbupperBoundx = aabb.upperBound.x + system!!.m_particleDiameter + val aabbupperBoundy = aabb.upperBound.y + system!!.m_particleDiameter + val firstProxy = lowerBound( + system!!.m_proxyBuffer, + system!!.m_proxyCount, + computeTag(system!!.m_inverseDiameter * aabblowerBoundx, system!!.m_inverseDiameter * aabblowerBoundy)) + val lastProxy = upperBound( + system!!.m_proxyBuffer, + system!!.m_proxyCount, + computeTag(system!!.m_inverseDiameter * aabbupperBoundx, system!!.m_inverseDiameter * aabbupperBoundy)) + + for (proxy in firstProxy until lastProxy) { + val a = system!!.m_proxyBuffer[proxy].index + val ap = system!!.m_positionBuffer.data!![a] + if (aabblowerBoundx <= ap.x && ap.x <= aabbupperBoundx && aabblowerBoundy <= ap.y + && ap.y <= aabbupperBoundy) { + val av = system!!.m_velocityBuffer.data!![a] + val temp = tempVec + Transform.mulTransToOutUnsafe(body!!.xf0, ap, temp) + Transform.mulToOutUnsafe(body.xf, temp, input.p1) + input.p2.x = ap.x + step!!.dt * av.x + input.p2.y = ap.y + step!!.dt * av.y + input.maxFraction = 1f + if (fixture.raycast(output, input, childIndex)) { + val p = tempVec + p.x = ((1 - output.fraction) * input.p1.x + output.fraction * input.p2.x + + Settings.linearSlop * output.normal.x) + p.y = ((1 - output.fraction) * input.p1.y + output.fraction * input.p2.y + + Settings.linearSlop * output.normal.y) + + val vx = step!!.inv_dt * (p.x - ap.x) + val vy = step!!.inv_dt * (p.y - ap.y) + av.x = vx + av.y = vy + val particleMass = system!!.particleMass + val ax = particleMass * (av.x - vx) + val ay = particleMass * (av.y - vy) + val b = output.normal + val fdn = ax * b.x + ay * b.y + val f = tempVec2 + f.x = fdn * b.x + f.y = fdn * b.y + body.applyLinearImpulse(f, p, true) + } + } + } + } + return true + } + } + + internal object Test { + fun IsProxyInvalid(proxy: Proxy): Boolean { + return proxy.index < 0 + } + + fun IsContactInvalid(contact: ParticleContact): Boolean { + return contact.indexA < 0 || contact.indexB < 0 + } + + fun IsBodyContactInvalid(contact: ParticleBodyContact): Boolean { + return contact.index < 0 + } + + fun IsPairInvalid(pair: Pair): Boolean { + return pair.indexA < 0 || pair.indexB < 0 + } + + fun IsTriadInvalid(triad: Triad): Boolean { + return triad.indexA < 0 || triad.indexB < 0 || triad.indexC < 0 + } + } + + companion object { + /** All particle types that require creating pairs */ + private val k_pairFlags = ParticleType.b2_springParticle + /** All particle types that require creating triads */ + private val k_triadFlags = ParticleType.b2_elasticParticle + /** All particle types that require computing depth */ + private val k_noPressureFlags = ParticleType.b2_powderParticle + + internal val xTruncBits = 12 + internal val yTruncBits = 12 + internal val tagBits = 8 * 4 - 1 /* sizeof(int) */ + internal val yOffset = (1 shl yTruncBits - 1).toLong() + internal val yShift = tagBits - yTruncBits + internal val xShift = tagBits - yTruncBits - xTruncBits + internal val xScale = (1 shl xShift).toLong() + internal val xOffset = xScale * (1 shl xTruncBits - 1) + internal val xMask = (1 shl xTruncBits) - 1 + internal val yMask = (1 shl yTruncBits) - 1 + + internal fun computeTag(x: Float, y: Float): Long { + return ((y + yOffset).toLong() shl yShift) + ((xScale * x).toLong() + xOffset) + } + + internal fun computeRelativeTag(tag: Long, x: Int, y: Int): Long { + return tag + (y shl yShift).toLong() + (x shl xShift).toLong() + } + + internal fun limitCapacity(capacity: Int, maxCount: Int): Int { + return if (maxCount != 0 && capacity > maxCount) maxCount else capacity + } + + private fun lowerBound(ray: Array, length: Int, tag: Long): Int { + var length = length + var left = 0 + var step: Int + var curr: Int + while (length > 0) { + step = length / 2 + curr = left + step + if (ray[curr].tag < tag) { + left = curr + 1 + length -= step + 1 + } else { + length = step + } + } + return left + } + + private fun upperBound(ray: Array, length: Int, tag: Long): Int { + var length = length + var left = 0 + var step: Int + var curr: Int + while (length > 0) { + step = length / 2 + curr = left + step + if (ray[curr].tag <= tag) { + left = curr + 1 + length -= step + 1 + } else { + length = step + } + } + return left + } + + // reallocate a buffer + internal fun reallocateBuffer(buffer: ParticleBuffer, oldCapacity: Int, newCapacity: Int, + deferred: Boolean): Array { + assert(newCapacity > oldCapacity) + return BufferUtils.reallocateBuffer(buffer.dataClass, buffer.data, buffer.userSuppliedCapacity, + oldCapacity, newCapacity, deferred) + } + + internal fun reallocateBuffer(buffer: ParticleBufferInt, oldCapacity: Int, newCapacity: Int, + deferred: Boolean): IntArray { + assert(newCapacity > oldCapacity) + return BufferUtils.reallocateBuffer(buffer.data, buffer.userSuppliedCapacity, oldCapacity, + newCapacity, deferred) + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleType.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleType.kt new file mode 100644 index 0000000..034623f --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/ParticleType.kt @@ -0,0 +1,38 @@ +package org.jbox2d.particle + +/** + * The particle type. Can be combined with | operator. Zero means liquid. + * + * @author dmurph + */ +object ParticleType { + + val b2_waterParticle = 0 + /** removed after next step */ + + val b2_zombieParticle = 1 shl 1 + /** zero velocity */ + + val b2_wallParticle = 1 shl 2 + /** with restitution from stretching */ + + val b2_springParticle = 1 shl 3 + /** with restitution from deformation */ + + val b2_elasticParticle = 1 shl 4 + /** with viscosity */ + + val b2_viscousParticle = 1 shl 5 + /** without isotropic pressure */ + + val b2_powderParticle = 1 shl 6 + /** with surface tension */ + + val b2_tensileParticle = 1 shl 7 + /** mixing color between contacting particles */ + + val b2_colorMixingParticle = 1 shl 8 + /** call b2DestructionListener on destruction */ + + val b2_destructionListener = 1 shl 9 +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/StackQueue.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/StackQueue.kt new file mode 100644 index 0000000..be57b5a --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/StackQueue.kt @@ -0,0 +1,45 @@ +package org.jbox2d.particle + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + + +class StackQueue { + + private var m_buffer: Array? = null + private var m_front: Int = 0 + private var m_back: Int = 0 + private var m_end: Int = 0 + + fun reset(buffer: Array) { + m_buffer = buffer + m_front = 0 + m_back = 0 + m_end = buffer.size + } + + fun push(task: T) { + if (m_back >= m_end) { + arraycopy(m_buffer!!, m_front, m_buffer!!, 0, m_back - m_front) + m_back -= m_front + m_front = 0 + if (m_back >= m_end) { + return + } + } + m_buffer!![m_back++] = task + } + + fun pop(): T { + assert(m_front < m_back) + return m_buffer!![m_front++] + } + + fun empty(): Boolean { + return m_front >= m_back + } + + fun front(): T { + return m_buffer!![m_front] + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/VoronoiDiagram.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/VoronoiDiagram.kt new file mode 100644 index 0000000..893b495 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/particle/VoronoiDiagram.kt @@ -0,0 +1,216 @@ +package org.jbox2d.particle + +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert +import org.jbox2d.pooling.normal.MutableStack + +class VoronoiDiagram(generatorCapacity: Int) { + + private val m_generatorBuffer: Array = Array(generatorCapacity) { Generator() } + private var m_generatorCount: Int = 0 + private var m_countX: Int = 0 + private var m_countY: Int = 0 + // The diagram is an array of "pointers". + private var m_diagram: Array? = null + + private val lower = Vec2() + private val upper = Vec2() + /* + e: java.lang.IllegalStateException: Cannot get FQ name of local class: class : org.jbox2d.pooling.normal.MutableStack defined in private final val taskPool: defined in org.jbox2d.particle.VoronoiDiagram + at org.jetbrains.kotlin.serialization.DescriptorAwareStringTable$DefaultImpls.getFqNameIndex(DescriptorAwareStringTable.kt:23) + at org.jetbrains.kotlin.serialization.StringTableImpl.getFqNameIndex(StringTableImpl.kt:25) + at org.jetbrains.kotlin.serialization.DescriptorSerializer.getClassifierId(DescriptorSerializer.kt:718) + at org.jetbrains.kotlin.serialization.DescriptorSerializer.fillFromPossiblyInnerType(DescriptorSerializer.kt:590) + at org.jetbrains.kotlin.serialization.DescriptorSerializer.type$serialization(DescriptorSerializer.kt:555) + at org.jetbrains.kotlin.serialization.DescriptorSerializer.typeId$serialization(DescriptorSerializer.kt:520) + at org.jetbrains.kotlin.serialization.DescriptorSerializer.propertyProto(DescriptorSerializer.kt:242) + */ + //private val taskPool = object : MutableStack(50) { + // override fun newInstance(): VoronoiDiagramTask = VoronoiDiagramTask() + // override fun newArray(size: Int): Array = + // arrayOfNulls(size) as Array + //} + + val taskPool: VoronoiDiagramTaskMutableStack = VoronoiDiagramTaskMutableStack() + + class VoronoiDiagramTaskMutableStack : MutableStack(50) { + override fun newInstance(): VoronoiDiagramTask = VoronoiDiagramTask() + override fun newArray(size: Int): Array = + arrayOfNulls(size) as Array + } + + private val queue = StackQueue() + + class Generator { + internal val center = Vec2() + internal var tag: Int = 0 + } + + class VoronoiDiagramTask { + internal var m_x: Int = 0 + internal var m_y: Int = 0 + internal var m_i: Int = 0 + internal var m_generator: Generator? = null + + constructor() {} + + constructor(x: Int, y: Int, i: Int, g: Generator) { + m_x = x + m_y = y + m_i = i + m_generator = g + } + + fun set(x: Int, y: Int, i: Int, g: Generator): VoronoiDiagramTask { + m_x = x + m_y = y + m_i = i + m_generator = g + return this + } + } + + interface VoronoiDiagramCallback { + fun callback(aTag: Int, bTag: Int, cTag: Int) + } + + fun getNodes(callback: VoronoiDiagramCallback) { + for (y in 0 until m_countY - 1) { + for (x in 0 until m_countX - 1) { + val i = x + y * m_countX + val a = m_diagram!![i] + val b = m_diagram!![i + 1] + val c = m_diagram!![i + m_countX] + val d = m_diagram!![i + 1 + m_countX] + if (b !== c) { + if (a !== b && a !== c) { + callback.callback(a!!.tag, b!!.tag, c!!.tag) + } + if (d !== b && d !== c) { + callback.callback(b!!.tag, d!!.tag, c!!.tag) + } + } + } + } + } + + fun addGenerator(center: Vec2, tag: Int) { + val g = m_generatorBuffer[m_generatorCount++] + g.center.x = center.x + g.center.y = center.y + g.tag = tag + } + + fun generate(radius: Float) { + assert(m_diagram == null) + val inverseRadius = 1 / radius + lower.x = Float.MAX_VALUE + lower.y = Float.MAX_VALUE + upper.x = -Float.MAX_VALUE + upper.y = -Float.MAX_VALUE + for (k in 0 until m_generatorCount) { + val g = m_generatorBuffer[k] + Vec2.minToOut(lower, g.center, lower) + Vec2.maxToOut(upper, g.center, upper) + } + m_countX = 1 + (inverseRadius * (upper.x - lower.x)).toInt() + m_countY = 1 + (inverseRadius * (upper.y - lower.y)).toInt() + m_diagram = arrayOfNulls(m_countX * m_countY) + queue.reset(arrayOfNulls(4 * m_countX * m_countX) as Array) + for (k in 0 until m_generatorCount) { + val g = m_generatorBuffer[k] + g.center.x = inverseRadius * (g.center.x - lower.x) + g.center.y = inverseRadius * (g.center.y - lower.y) + val x = MathUtils.max(0, MathUtils.min(g.center.x.toInt(), m_countX - 1)) + val y = MathUtils.max(0, MathUtils.min(g.center.y.toInt(), m_countY - 1)) + queue.push(taskPool.pop().set(x, y, x + y * m_countX, g)) + } + while (!queue.empty()) { + val front = queue.pop() + val x = front.m_x + val y = front.m_y + val i = front.m_i + val g = front.m_generator + if (m_diagram!![i] == null) { + m_diagram!![i] = g!! + if (x > 0) { + queue.push(taskPool.pop().set(x - 1, y, i - 1, g)) + } + if (y > 0) { + queue.push(taskPool.pop().set(x, y - 1, i - m_countX, g)) + } + if (x < m_countX - 1) { + queue.push(taskPool.pop().set(x + 1, y, i + 1, g)) + } + if (y < m_countY - 1) { + queue.push(taskPool.pop().set(x, y + 1, i + m_countX, g)) + } + } + taskPool.push(front) + } + val maxIteration = m_countX + m_countY + for (iteration in 0 until maxIteration) { + for (y in 0 until m_countY) { + for (x in 0 until m_countX - 1) { + val i = x + y * m_countX + val a = m_diagram!![i]!! + val b = m_diagram!![i + 1]!! + if (a !== b) { + queue.push(taskPool.pop().set(x, y, i, b)) + queue.push(taskPool.pop().set(x + 1, y, i + 1, a)) + } + } + } + for (y in 0 until m_countY - 1) { + for (x in 0 until m_countX) { + val i = x + y * m_countX + val a = m_diagram!![i]!! + val b = m_diagram!![i + m_countX]!! + if (a !== b) { + queue.push(taskPool.pop().set(x, y, i, b)) + queue.push(taskPool.pop().set(x, y + 1, i + m_countX, a)) + } + } + } + var updated = false + while (!queue.empty()) { + val front = queue.pop() + val x = front.m_x + val y = front.m_y + val i = front.m_i + val k = front.m_generator + val a = m_diagram!![i] + val b = k + if (a !== b) { + val ax = a!!.center.x - x + val ay = a!!.center.y - y + val bx = b!!.center.x - x + val by = b.center.y - y + val a2 = ax * ax + ay * ay + val b2 = bx * bx + by * by + if (a2 > b2) { + m_diagram!![i] = b + if (x > 0) { + queue.push(taskPool.pop().set(x - 1, y, i - 1, b)) + } + if (y > 0) { + queue.push(taskPool.pop().set(x, y - 1, i - m_countX, b)) + } + if (x < m_countX - 1) { + queue.push(taskPool.pop().set(x + 1, y, i + 1, b)) + } + if (y < m_countY - 1) { + queue.push(taskPool.pop().set(x, y + 1, i + m_countX, b)) + } + updated = true + } + } + taskPool.push(front) + } + if (!updated) { + break + } + } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IDynamicStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IDynamicStack.kt new file mode 100644 index 0000000..8cdad9b --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IDynamicStack.kt @@ -0,0 +1,47 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling + +/** + * Same functionality of a regular java.util stack. Object + * return order does not matter. + * @author Daniel + * + * @param + */ +interface IDynamicStack { + + /** + * Pops an item off the stack + * @return + */ + fun pop(): E + + /** + * Pushes an item back on the stack + * @param argObject + */ + fun push(argObject: E) + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IOrderedStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IOrderedStack.kt new file mode 100644 index 0000000..1735587 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IOrderedStack.kt @@ -0,0 +1,57 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling + +/** + * This stack assumes that when you push 'n' items back, + * you're pushing back the last 'n' items popped. + * @author Daniel + * + * @param + */ +interface IOrderedStack { + + /** + * Returns the next object in the pool + * @return + */ + fun pop(): E + + /** + * Returns the next 'argNum' objects in the pool + * in an array + * @param argNum + * @return an array containing the next pool objects in + * items 0-argNum. Array length and uniqueness not + * guaranteed. + */ + fun pop(argNum: Int): Array + + /** + * Tells the stack to take back the last 'argNum' items + * @param argNum + */ + fun push(argNum: Int) + +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IWorldPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IWorldPool.kt new file mode 100644 index 0000000..bbff1ed --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/IWorldPool.kt @@ -0,0 +1,100 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.Collision +import org.jbox2d.collision.Distance +import org.jbox2d.collision.TimeOfImpact +import org.jbox2d.common.Mat22 +import org.jbox2d.common.Mat33 +import org.jbox2d.common.Rot +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import org.jbox2d.dynamics.contacts.Contact + +/** + * World pool interface + * @author Daniel + */ +interface IWorldPool { + + val polyContactStack: IDynamicStack + + val circleContactStack: IDynamicStack + + val polyCircleContactStack: IDynamicStack + + val edgeCircleContactStack: IDynamicStack + + val edgePolyContactStack: IDynamicStack + + val chainCircleContactStack: IDynamicStack + + val chainPolyContactStack: IDynamicStack + + val collision: Collision + + val timeOfImpact: TimeOfImpact + + val distance: Distance + + fun popVec2(): Vec2 + + fun popVec2(num: Int): Array + + fun pushVec2(num: Int) + + fun popVec3(): Vec3 + + fun popVec3(num: Int): Array + + fun pushVec3(num: Int) + + fun popMat22(): Mat22 + + fun popMat22(num: Int): Array + + fun pushMat22(num: Int) + + fun popMat33(): Mat33 + + fun pushMat33(num: Int) + + fun popAABB(): AABB + + fun popAABB(num: Int): Array + + fun pushAABB(num: Int) + + fun popRot(): Rot + + fun pushRot(num: Int) + + fun getFloatArray(argLength: Int): FloatArray + + fun getIntArray(argLength: Int): IntArray + + fun getVec2Array(argLength: Int): Array +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/FloatArrayPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/FloatArrayPool.kt new file mode 100644 index 0000000..0161056 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/FloatArrayPool.kt @@ -0,0 +1,50 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling.arrays + +import org.jbox2d.internal.assert + +/** + * Not thread safe float[] pooling. + * @author Daniel + */ +class FloatArrayPool { + + private val map = HashMap() + + operator fun get(argLength: Int): FloatArray { + assert(argLength > 0) + + if (!map.containsKey(argLength)) { + map[argLength] = getInitializedArray(argLength) + } + + assert(map[argLength]!!.size == argLength) { "Array not built of correct length" } + return map[argLength]!! + } + + protected fun getInitializedArray(argLength: Int): FloatArray { + return FloatArray(argLength) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/GeneratorArrayPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/GeneratorArrayPool.kt new file mode 100644 index 0000000..2d1d35a --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/GeneratorArrayPool.kt @@ -0,0 +1,24 @@ +package org.jbox2d.pooling.arrays + +import org.jbox2d.internal.assert +import org.jbox2d.particle.VoronoiDiagram + +class GeneratorArrayPool { + + private val map = HashMap>() + + operator fun get(length: Int): Array { + assert(length > 0) + + if (!map.containsKey(length)) { + map[length] = getInitializedArray(length) + } + + assert(map[length]!!.size == length) { "Array not built of correct length" } + return map[length]!! + } + + protected fun getInitializedArray(length: Int): Array { + return Array(length) { VoronoiDiagram.Generator() } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/IntArrayPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/IntArrayPool.kt new file mode 100644 index 0000000..0c1c9e8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/IntArrayPool.kt @@ -0,0 +1,53 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:14:34 AM Jul 17, 2010 + */ +package org.jbox2d.pooling.arrays + +import org.jbox2d.internal.assert + +/** + * Not thread safe int[] pooling + * @author Daniel Murphy + */ +class IntArrayPool { + + private val map = HashMap() + + operator fun get(argLength: Int): IntArray { + assert(argLength > 0) + + if (!map.containsKey(argLength)) { + map[argLength] = getInitializedArray(argLength) + } + + assert(map[argLength]!!.size == argLength) { "Array not built of correct length" } + return map[argLength]!! + } + + protected fun getInitializedArray(argLength: Int): IntArray { + return IntArray(argLength) + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/Vec2ArrayPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/Vec2ArrayPool.kt new file mode 100644 index 0000000..64af92e --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/arrays/Vec2ArrayPool.kt @@ -0,0 +1,51 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling.arrays + +import org.jbox2d.common.Vec2 +import org.jbox2d.internal.assert + +/** + * not thread safe Vec2[] pool + * @author dmurph + */ +class Vec2ArrayPool { + + private val map = HashMap>() + + operator fun get(argLength: Int): Array { + assert(argLength > 0) + + if (!map.containsKey(argLength)) { + map[argLength] = getInitializedArray(argLength) + } + + assert(map[argLength]!!.size == argLength) { "Array not built of correct length" } + return map[argLength]!! + } + + protected fun getInitializedArray(argLength: Int): Array { + return Array(argLength) { Vec2() } + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/CircleStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/CircleStack.kt new file mode 100644 index 0000000..5016583 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/CircleStack.kt @@ -0,0 +1,62 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling.normal + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IOrderedStack + +abstract class CircleStack(private val size: Int, argContainerSize: Int) : IOrderedStack { + + private val pool: Array = Array(size) { newInstance() as Any } + private var index: Int = 0 + private val container: Array = Array(argContainerSize) { } + + override fun pop(): E { + index++ + if (index >= size) { + index = 0 + } + return pool[index] as E + } + + override fun pop(argNum: Int): Array { + assert(argNum <= container.size) { "Container array is too small" } + if (index + argNum < size) { + arraycopy(pool, index, container, 0, argNum) + index += argNum + } else { + val overlap = index + argNum - size + arraycopy(pool, index, container, 0, argNum - overlap) + arraycopy(pool, 0, container, argNum - overlap, overlap) + index = overlap + } + return container as Array + } + + override fun push(argNum: Int) {} + + /** Creates a new instance of the object contained by this stack. */ + protected abstract fun newInstance(): E +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/DefaultWorldPool.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/DefaultWorldPool.kt new file mode 100644 index 0000000..60030b8 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/DefaultWorldPool.kt @@ -0,0 +1,197 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 3:26:14 AM Jan 11, 2011 + */ +package org.jbox2d.pooling.normal + +import org.jbox2d.collision.AABB +import org.jbox2d.collision.Collision +import org.jbox2d.collision.Distance +import org.jbox2d.collision.TimeOfImpact +import org.jbox2d.common.Mat22 +import org.jbox2d.common.Mat33 +import org.jbox2d.common.Rot +import org.jbox2d.common.Settings +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import org.jbox2d.dynamics.contacts.ChainAndCircleContact +import org.jbox2d.dynamics.contacts.ChainAndPolygonContact +import org.jbox2d.dynamics.contacts.CircleContact +import org.jbox2d.dynamics.contacts.Contact +import org.jbox2d.dynamics.contacts.EdgeAndCircleContact +import org.jbox2d.dynamics.contacts.EdgeAndPolygonContact +import org.jbox2d.dynamics.contacts.PolygonAndCircleContact +import org.jbox2d.dynamics.contacts.PolygonContact +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IDynamicStack +import org.jbox2d.pooling.IWorldPool + +/** + * Provides object pooling for all objects used in the engine. Objects retrieved from here should + * only be used temporarily, and then pushed back (with the exception of arrays). + * + * @author Daniel Murphy + */ +class DefaultWorldPool(argSize: Int, argContainerSize: Int) : IWorldPool { + private val world = this + + private val vecs: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { Vec2() } + private val vec3s: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { Vec3() } + private val mats: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { Mat22() } + private val mat33s: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { Mat33() } + private val aabbs: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { AABB() } + private val rots: OrderedStack = LambdaOrderedStack(argSize, argContainerSize) { Rot() } + + private val afloats = HashMap() + private val aints = HashMap() + private val avecs = HashMap>() + + private val pcstack = ContactMutableStack({ PolygonContact(world) }, { arrayOfNulls(it) }) + private val ccstack = ContactMutableStack({ CircleContact(world) }, { arrayOfNulls(it) }) + private val cpstack = ContactMutableStack({ PolygonAndCircleContact(world) }, { arrayOfNulls(it) }) + private val ecstack = ContactMutableStack({ EdgeAndCircleContact(world) }, { arrayOfNulls(it) }) + private val epstack = ContactMutableStack({ EdgeAndPolygonContact(world) }, { arrayOfNulls(it) }) + private val chcstack = ContactMutableStack({ ChainAndCircleContact(world) }, { arrayOfNulls(it) }) + private val chpstack = ContactMutableStack({ ChainAndPolygonContact(world) }, { arrayOfNulls(it) }) + + override val collision: Collision = Collision(this) + override val timeOfImpact: TimeOfImpact = TimeOfImpact(this) + override val distance: Distance = Distance() + + override val polyContactStack: IDynamicStack get() = pcstack + override val circleContactStack: IDynamicStack get() = ccstack + override val polyCircleContactStack: IDynamicStack get() = cpstack + override val edgeCircleContactStack: IDynamicStack get() = ecstack + override val edgePolyContactStack: IDynamicStack get() = epstack + override val chainCircleContactStack: IDynamicStack get() = chcstack + override val chainPolyContactStack: IDynamicStack get() = chpstack + + override fun popVec2(): Vec2 { + return vecs.pop() + } + + override fun popVec2(argNum: Int): Array { + return vecs.pop(argNum) + } + + override fun pushVec2(argNum: Int) { + vecs.push(argNum) + } + + override fun popVec3(): Vec3 { + return vec3s.pop() + } + + override fun popVec3(argNum: Int): Array { + return vec3s.pop(argNum) + } + + override fun pushVec3(argNum: Int) { + vec3s.push(argNum) + } + + override fun popMat22(): Mat22 { + return mats.pop() + } + + override fun popMat22(argNum: Int): Array { + return mats.pop(argNum) + } + + override fun pushMat22(argNum: Int) { + mats.push(argNum) + } + + override fun popMat33(): Mat33 { + return mat33s.pop() + } + + override fun pushMat33(argNum: Int) { + mat33s.push(argNum) + } + + override fun popAABB(): AABB { + return aabbs.pop() + } + + override fun popAABB(argNum: Int): Array { + return aabbs.pop(argNum) + } + + override fun pushAABB(argNum: Int) { + aabbs.push(argNum) + } + + override fun popRot(): Rot { + return rots.pop() + } + + override fun pushRot(num: Int) { + rots.push(num) + } + + override fun getFloatArray(argLength: Int): FloatArray { + if (!afloats.containsKey(argLength)) { + afloats[argLength] = FloatArray(argLength) + } + + assert(afloats[argLength]!!.size == argLength) { "Array not built with correct length" } + return afloats[argLength]!! + } + + override fun getIntArray(argLength: Int): IntArray { + if (!aints.containsKey(argLength)) { + aints[argLength] = IntArray(argLength) + } + + assert(aints[argLength]!!.size == argLength) { "Array not built with correct length" } + return aints[argLength]!! + } + + override fun getVec2Array(argLength: Int): Array { + if (!avecs.containsKey(argLength)) { + avecs[argLength] = Array(argLength) { Vec2() } + } + + assert(avecs[argLength]!!.size == argLength) { "Array not built with correct length" } + return avecs[argLength]!! + } +} + +class LambdaOrderedStack(argSize: Int, argContainerSize: Int, val newInstanceLambda: () -> T) : OrderedStack(argSize, argContainerSize) { + override fun newInstance(): T { + //println("newInstanceLambda:$newInstanceLambda") + return newInstanceLambda() + } +} + +class ContactMutableStack(private val newInstanceLambda: () -> Contact, val newArrayLambda: (Int) -> Array) : MutableStack(Settings.CONTACT_STACK_INIT_SIZE) { + override fun newInstance(): Contact = newInstanceLambda() + override fun newArray(size: Int): Array { + //println("ContactMutableStack.newArray: $this, $newInstanceLambda, $newArrayLambda") + return newArrayLambda(size) as Array + } +} + diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/MutableStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/MutableStack.kt new file mode 100644 index 0000000..aa590ba --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/MutableStack.kt @@ -0,0 +1,71 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling.normal + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert +import org.jbox2d.pooling.IDynamicStack + +abstract class MutableStack(private val argInitSize: Int) : IDynamicStack { + private var stack: Array? = null + private var index: Int = 0 + private var size: Int = 0 + + private fun ensureInit() { + if (stack == null) { + extendStack(argInitSize) + } + } + + private fun extendStack(argSize: Int) { + val newStack = newArray(argSize) + if (stack != null) { + arraycopy(stack!!, 0, newStack, 0, size) + } + for (i in newStack.indices) { + newStack[i] = newInstance() + } + stack = newStack + size = newStack.size + } + + override fun pop(): E { + ensureInit() + if (index >= size) { + extendStack(size * 2) + } + return stack!![index++] + } + + override fun push(argObject: E) { + ensureInit() + assert(index > 0) + stack!![--index] = argObject + } + + /** Creates a new instance of the object contained by this stack. */ + protected abstract fun newInstance(): E + + protected abstract fun newArray(size: Int): Array +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/OrderedStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/OrderedStack.kt new file mode 100644 index 0000000..e59ca67 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/normal/OrderedStack.kt @@ -0,0 +1,60 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 12:52:04 AM Jan 20, 2011 + */ +package org.jbox2d.pooling.normal + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + +/** + * @author Daniel Murphy + */ +abstract class OrderedStack(private val size: Int, argContainerSize: Int) { + private val pool: Array by lazy { Array(size) { newInstance() as Any } } + private var index: Int = 0 + private val container: Array = Array(argContainerSize) { } + + fun pop(): E { + assert(index < size) { "End of stack reached, there is probably a leak somewhere" } + return pool[index++] as E + } + + fun pop(argNum: Int): Array { + assert(index + argNum < size) { "End of stack reached, there is probably a leak somewhere" } + assert(argNum <= container.size) { "Container array is too small" } + arraycopy(pool, index, container, 0, argNum) + index += argNum + return container as Array + } + + fun push(argNum: Int) { + index -= argNum + assert(index >= 0) { "Beginning of stack reached, push/pops are unmatched" } + } + + /** Creates a new instance of the object contained by this stack. */ + protected abstract fun newInstance(): E +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/stacks/DynamicIntStack.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/stacks/DynamicIntStack.kt new file mode 100644 index 0000000..52b2777 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/pooling/stacks/DynamicIntStack.kt @@ -0,0 +1,58 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +package org.jbox2d.pooling.stacks + +import org.jbox2d.internal.arraycopy +import org.jbox2d.internal.assert + +class DynamicIntStack(private var size: Int) { + + private var stack: IntArray? = null + var count: Int = 0 + private set + + init { + stack = IntArray(size) + count = 0 + } + + fun reset() { + count = 0 + } + + fun pop(): Int { + assert(count > 0) + return stack!![--count] + } + + fun push(i: Int) { + if (count == size) { + val old = stack + stack = IntArray(size * 2) + size = stack!!.size + arraycopy(old!!, 0, stack!!, 0, old.size) + } + stack!![count++] = i + } +} diff --git a/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/userdata/Box2dTypedUserData.kt b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/userdata/Box2dTypedUserData.kt new file mode 100644 index 0000000..91945e3 --- /dev/null +++ b/modules/korge-box2d/src/commonMain/kotlin/org/jbox2d/userdata/Box2dTypedUserData.kt @@ -0,0 +1,24 @@ +package org.jbox2d.userdata + +interface Box2dTypedUserData { + @kotlin.Suppress("unused") + open class Key + + operator fun contains(key: Key): Boolean + operator fun get(key: Key): T? + operator fun set(key: Key, value: T?) + + class Mixin : Box2dTypedUserData { + private var typedUserData: LinkedHashMap, Any>? = null + override operator fun contains(key: Key): Boolean = typedUserData?.containsKey(key) == true + override operator fun get(key: Key): T? = typedUserData?.get(key) as T? + override operator fun set(key: Key, value: T?) { + if (value != null) { + if (typedUserData == null) typedUserData = LinkedHashMap() + typedUserData?.set(key, value) + } else { + typedUserData?.remove(key) + } + } + } +} diff --git a/modules/korge-box2d/src/commonTest/kotlin/korlibs/korge/box2d/Box2dTest.kt b/modules/korge-box2d/src/commonTest/kotlin/korlibs/korge/box2d/Box2dTest.kt new file mode 100644 index 0000000..3511c7c --- /dev/null +++ b/modules/korge-box2d/src/commonTest/kotlin/korlibs/korge/box2d/Box2dTest.kt @@ -0,0 +1,71 @@ +package korlibs.korge.box2d + +import korlibs.korge.tests.ViewsForTesting +import korlibs.korge.view.* +import korlibs.math.geom.Anchor +import korlibs.time.milliseconds +import org.jbox2d.dynamics.BodyType +import kotlin.test.Test +import kotlin.test.assertEquals + +class Box2dTest : ViewsForTesting() { + @Test + fun testSetInitialLinearVelocity() { + val container = Container() + val rect = container + .solidRect(920, 100).xy(0, 620) + .registerBodyWithFixture(type = BodyType.STATIC, friction = 0.2, restitution = 0.2) + val n = 0 + val c = container.circle(50f) + //ellipse(Size(50, 50)) + .xy(120 + 140 * n, 246) + .anchor(Anchor.CENTER) + .registerBodyWithFixture( + type = BodyType.DYNAMIC, + linearVelocityX = 16.0, + friction = 0.2, + restitution = 0.3 + (n * 0.1) + ) + c.body!!._linearVelocity.x = 16f + assertEquals(120.0, c.pos.x.toDouble(), 0.1) + assertEquals(246.0, c.pos.y.toDouble(), 0.1) + container.updateSingleView(1000.milliseconds) + assertEquals(196.79994, c.pos.x.toDouble(), 0.1) + assertEquals(252.77376, c.pos.y.toDouble(), 0.1) + } + + @Test + fun test() = viewsTest { + /* + lateinit var body: Body + + val view = worldView { + createBody { + setPosition(0, -10) + }.fixture { + shape = BoxShape(100, 20) + density = 0f + }.setView(graphics { + fill(Colors.RED) { + rect(-50f, -10f, 100f, 20f) + //anchor(0.5, 0.5) + } + }) + + // Dynamic Body + body = createBody { + type = BodyType.DYNAMIC + setPosition(0, 10) + }.fixture { + shape = BoxShape(2f, 2f) + density = 1f + friction = .2f + }.setView(solidRect(2f, 2f, Colors.GREEN).anchor(.5, .5)) + } + + assertEquals(10f, body.position.y) + for (n in 0 until 40) view.updateSingleView(16.milliseconds) + assertEquals(true, body.position.y < 8f) + */ + } +} diff --git a/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/MathTest.kt b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/MathTest.kt new file mode 100644 index 0000000..34944c9 --- /dev/null +++ b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/MathTest.kt @@ -0,0 +1,160 @@ +/******************************************************************************* + * Copyright (c) 2013, Daniel Murphy + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * Created at 4:32:38 AM Jan 14, 2011 + */ +package org.jbox2d.utests + +import org.jbox2d.common.Mat22 +import org.jbox2d.common.Mat33 +import org.jbox2d.common.MathUtils +import org.jbox2d.common.Vec2 +import org.jbox2d.common.Vec3 +import kotlin.math.abs +import kotlin.math.ceil +import kotlin.math.floor +import kotlin.math.round +import kotlin.random.Random +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertNotEquals + +/** + * @author Daniel Murphy + */ +class MathTest { + val r = Random(0) + + @Test + fun testFastMathFloor() { + for (i in 0 until RAND_ITERS) { + val a = r.nextFloat() * MAX - MAX / 2 + assertEquals(floor(a.toDouble()).toInt(), MathUtils.floor(a)) + } + } + + @Test + fun testFastMathCeil() { + for (i in 0 until RAND_ITERS) { + val a = r.nextFloat() * MAX - MAX / 2 + assertEquals(ceil(a.toDouble()).toInt(), MathUtils.ceil(a)) + } + } + + // Fails on: :kbox2d:jsIrNodeTest + //@Test + //fun testFastMathMax() { + // for (i in 0 until RAND_ITERS) { + // val a = r.nextFloat() * MAX - MAX / 2 + // val b = r.nextFloat() * MAX - MAX / 2 + // assertEquals(max(a, b), MathUtils.max(a, b)) + // } + //} + // + //@Test + //fun testFastMathMin() { + // for (i in 0 until RAND_ITERS) { + // val a = r.nextFloat() * MAX - MAX / 2 + // val b = r.nextFloat() * MAX - MAX / 2 + // assertEquals(min(a, b), MathUtils.min(a, b)) + // } + //} + + @Test + fun testFastMathRound() { + for (i in 0 until RAND_ITERS) { + val a = r.nextFloat() * MAX - MAX / 2 + assertEquals(round(a).toInt(), MathUtils.round(a)) // Failed on watchOS X86 + } + } + + @Test + fun testFastMathAbs() { + for (i in 0 until RAND_ITERS) { + val a = r.nextFloat() * MAX - MAX / 2 + assertEquals(abs(a), MathUtils.abs(a)) + } + } + + fun testVec2() { + val v = Vec2() + v.x = 0f + v.y = 1f + v.subLocal(Vec2(10f, 10f)) + assertEquals(-10f, v.x) + assertEquals(-9f, v.y) + + val v2 = v.add(Vec2(1f, 1f)) + assertEquals(-9f, v2.x) + assertEquals(-8f, v2.y) + assertNotEquals(v, v2) + } + + fun testMat22Unsafes() { + val v1 = Vec2(10f, -1.3f) + val m1 = Mat22(1f, 34f, -3f, 3f) + val m2 = Mat22(2f, -1f, 4.1f, -4f) + val vo = Vec2() + val mo = Mat22() + + Mat22.mulToOutUnsafe(m1, m2, mo) + assertEquals(Mat22.mul(m1, m2), mo) + + Mat22.mulToOutUnsafe(m1, v1, vo) + assertEquals(Mat22.mul(m1, v1), vo) + + Mat22.mulTransToOutUnsafe(m1, m2, mo) + assertEquals(Mat22.mulTrans(m1, m2), mo) + + Mat22.mulTransToOutUnsafe(m1, v1, vo) + assertEquals(Mat22.mulTrans(m1, v1), vo) + } + + fun testMat33() { + val mat = Mat33() + + mat.ex.set(3f, 19f, -5f) + mat.ey.set(-1f, 1f, 4f) + mat.ez.set(-10f, 4f, 4f) + + val b = Vec3(4f, 1f, 2f) + assertEquals(Vec3(0.096f, 1.1013334f, -.48133332f), mat.solve33(b)) + + val b2 = Vec2(4f, 1f) + assertEquals(Vec2(0.22727273f, -3.318182f), mat.solve22(b2)) + } + + fun testVec3() { + val v1 = Vec3() + val v2 = Vec3() + + assertEquals(Vec3(1f, -15f, 36f), Vec3.cross(v1.set(9f, 3f, 1f), v2.set(3f, 5f, 2f))) + } + + companion object { + + private val MAX = (Float.MAX_VALUE / 1000).toInt() + private val RAND_ITERS = 100 + } +} diff --git a/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/PolygonShapeTest.kt b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/PolygonShapeTest.kt new file mode 100644 index 0000000..82ec7f6 --- /dev/null +++ b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/PolygonShapeTest.kt @@ -0,0 +1,22 @@ +package org.jbox2d.utests + +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.common.Vec2 +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertTrue + + +class PolygonShapeTest { + @Test + fun test() { + val shape = PolygonShape() + val vertexArray = arrayOf(Vec2(1f, 1f), Vec2(1f, 0f), Vec2(0f, 0f)) + shape.set(vertexArray, vertexArray.size) + + assertEquals(shape.count, vertexArray.size) + (0 until shape.count).forEach { + assertTrue(vertexArray.contains(shape.getVertex(it))) + } + } +} diff --git a/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/WorldTest.kt b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/WorldTest.kt new file mode 100644 index 0000000..2cc25f8 --- /dev/null +++ b/modules/korge-box2d/src/commonTest/kotlin/org/jbox2d/utests/WorldTest.kt @@ -0,0 +1,61 @@ +package org.jbox2d.utests + +import org.jbox2d.collision.shapes.PolygonShape +import org.jbox2d.common.Vec2 +import org.jbox2d.dynamics.BodyDef +import org.jbox2d.dynamics.BodyType +import org.jbox2d.dynamics.FixtureDef +import org.jbox2d.dynamics.World +import org.jbox2d.userdata.Box2dTypedUserData +import kotlin.test.Test +import kotlin.test.assertEquals + + +class WorldTest { + @Test + fun test() { + class Demo(val a: Int = 10) + + val DemoKey = Box2dTypedUserData.Key() + + val world = World(Vec2(0f, -10f)) + world[DemoKey] = Demo() + val groundBodyDef = BodyDef() + groundBodyDef.position.set(0f, -10f) + val groundBody = world.createBody(groundBodyDef) + val groundBox = PolygonShape() + groundBox.setAsBox(50f, 10f) + groundBody.createFixture(groundBox, 0f) + + // Dynamic Body + val bodyDef = BodyDef() + bodyDef.type = BodyType.DYNAMIC + bodyDef.position.set(0f, 4f) + val body = world.createBody(bodyDef) + val dynamicBox = PolygonShape() + dynamicBox.setAsBox(1f, 1f) + val fixtureDef = FixtureDef() + fixtureDef.shape = dynamicBox + fixtureDef.density = 1f + fixtureDef.friction = 0.3f + body.createFixture(fixtureDef) + + // Setup world + val timeStep = 1.0f / 60.0f + val velocityIterations = 6 + val positionIterations = 2 + + assertEquals(true, world[DemoKey] is Demo) + assertEquals(true, bodyDef[DemoKey] == null) + world[DemoKey] = null + assertEquals(false, world[DemoKey] is Demo) + + // Run loop + for (i in 0 until 60) { + world.step(timeStep, velocityIterations, positionIterations) + val position = body.position + val angle = body.angleRadians + println("${position.x} ${position.y} $angle") + } + } +} diff --git a/modules/korge-box2d/src/main/AndroidManifest.xml b/modules/korge-box2d/src/main/AndroidManifest.xml new file mode 100644 index 0000000..f5a341d --- /dev/null +++ b/modules/korge-box2d/src/main/AndroidManifest.xml @@ -0,0 +1,2 @@ +